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ABSTRACT 


Considerations  of  real-time  control  problems  for  an  Autonomous  Underwater 
Vehicle  (AUV)  are  addressed  in  this  research.  Among  these  problems  is  the  ability  to 
control  the  submersible  given  its  highly  nonlinear  operating  environment.  In  order  to 
account  for  these  variations,  robust  control  techniques  must  be  used.  In  particular. 
Variable  Structure  Control  (VSC)  with  Doyle-Stein  Observer  has  proven  to  produce 
optimal  results  while  maintaining  a  high  degree  of  robustness.  This  led  to  the 
development  of  a  real-time  error  detector  using  the  robust  observer  to  provide  system 
redundancy  through  software.  The  culmination  of  this  work  is  a  real-time  autopilot  written 
in  the  "C"  language  which  is  ready  for  implementation  and  testing  in  the  Naval 
Postgraduate  School  AUV  prototype. 

We  also  address  the  aspect  of  real-time  signal  processing  and  conditioning  in  terms 
of  Synchro-to-Resolver  Conversion  and  anti-aliasing  filters.  The  synchro  problem  involves 
converting  a  nonpotentiometric  directional  gyro  output  to  a  natural  binary  format  which 
calls  for  an  intricate  design  of  power  transformers,  analog-to-digital  converter,  and  passive 
element  components.  Lastly,  the  use  of  Generalized-Immittance  Converter  circuitry  in  the 
design  of  very  low  frequency  anti-aliasing  filter  applications  is  developed  and  tested. 
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I.  INTRODUCTION 


The  Autonomous  Underwater  Vehicle  (AUV)  project  was  conceived  at  the  Naval 
Postgraduate  School  in  the  early  1980’s.  The  purpose  of  the  project  was  the  development 
of  an  underwater  vehicle  as  a  testbed  for  research.  Most  of  the  early  work  on  this  project 
was  on  model  development  and  applications  of  the  hydrodynamic  equations  essential  in 
modeling  vehicle  motions  accurately.  More  recently  autonomous  navigation,  obstacle 
avoidance,  path  planning,  and  advanced  control  techniques  have  become  the  focus  of 
current  research,  in  particular  adaptive  and  nonlinear  control  design. 

To  get  an  understanding  of  the  goals  of  the  project  and  the  need  for  research  in  the 
aforementioned  areas,  a  basic  knowledge  of  the  concept  and  scope  of  operation  of  the 
AUV  is  needed.  The  NPS  vehicle  is  designed  to  be  an  untethered  submersible  capable 
of  autonomous  operation  for  an  extended  period  of  time.  Conceptually,  the  vehicle  is 
placed  in  an  unknown  environment  where  it  will  map  the  immediate  surrounding  area 
while  evading  static  and  dynamic  obstacles.  Upon  completion  of  its  mission,  the  vehicle 
will  return  to  its  base  for  retrieval  and  subsequent  down-loading  of  its  mission  data. 

The  submersible  must  be  capable  of  operating  in  various  dynamic  environments  in 
order  to  complete  this  type  of  mission.  This  can  be  achieved  by  adding  a  level  of 
intelligence,  at  the  expense  of  increased  complexity  of  the  real-time  control  problem. 

The  purpose  of  this  work  is  to  design  and  implement  a  real-time  control  system  for 
rapid  maneuvers  of  the  AUV.  The  techiiique  used  is  based  on  Variable  Structure  Control, 
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with  subsequent  computer  code  developed  in  a  high  level  language.  Controller 
requirements  are  in  terms  of  performance  (i.e.,  fast  response)  and  robustness  in  the 
presence  of  modeling  errors  and  sensor  failures. 

A  robust  observer  has  been  designed  for  the  purpose  of  redundancy  so  that  system 
stability  can  still  be  maintained  when  one  or  more  sensors  (pitch  and  pitch  rate,  for 
example)  fail.  Upon  successful  modeling  and  testing  of  this  complete  autopilot,  the  code 
was  converted  into  the  "C"  language  for  real-time  implementation.  The  converted  "C" 
code  then  was  tested  using  an  interface  module  that  emulates  real-time  sampling 
conditions  and  acts  as  a  link  between  model,  controller,  and  local  path  planner.  A  further 
aspect  of  this  work  is  the  design  and  testing  of  various  hardware  components  of  the 
electronic  signal  processing  and  conditioning  network  proposed  for  the  AUV. 

This  thesis  discusses  issues  ranging  from  automatic  control  systems  to  hardware 
implementation  of  electronic  signal  processing  equipment.  In  Chapter  II  the  autopilot 
control  principle  and  algorithm  are  developed.  Chapter  III  addresses  considerations  of 
real-time  code  development  and  testing.  In  Chapter  IV  hardware  design  issues  concerning 
signal  conditioning  and  information  encoding  are  covered.  The  final  chapter  contains 
conclusions  and  general  project  assessment.  All  pertinent  computer  code  and  hardware 
designs  are  located  in  the  appendices. 
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II.  REAL-TIME  CONTROL  METHODS  FOR  DEPTH  AND  STEERING 

A.  BACKGROUND  INFORMATION 

An  accurate  model  of  the  system  must  exist  in  order  to  develop  and  test  control 
algorithms.  This  model  should  allow  for  all  possible  dynamic  situations.  In  the  case  of 
the  AUV,  a  model  based  on  the  United  States  Navy’s  Swimmer  Delivery  Vehicle  is  used. 
This  model  is  discussed  at  length  in  Chapter  HI.  The  estimated  dynamics  of  the  prototype 
AUV  helped  to  determine  the  control  methodology  used  in  the  real-time  algorithms.  The 
uncertainties  in  the  dynamic  response  of  the  vehicle  due  to  the  operating  enviroment 
means  a  robust  control  scheme  has  to  be  used.  The  control  techniques  used  in  steering 
and  depth  are  distinctly  different  due  to  these  vehicle  dynamics  and  purposed  operating 
conditions.  This  chapter  discusses  the  development,  testing,  simulation,  and  future  real¬ 
time  implementation  of  an  autopilot  for  the  AUV.  The  derivation  in  this  chapter  of  the 
variable  structure  algorithm  is  based  on  work  by  Cristi  [Ref.  1]. 

B.  VARIABLE  STRUCTURE  CONTROL  ALGORITHM  DERIVATION 

This  technique  is  used  in  the  design  of  regulators  for  nonlinear  time  varying 
uncertain  systems  [Ref.  2],  Variable  structure  control,  commonly  referred  to  as  sliding 
mode  control,  can  be  based  on  a  nominal  model,  and  it  can  account  for  uncertainties  in 
the  dynamic  response  of  the  plant.  The  main  feature  of  this  method  of  control  is  the  fact 
that  it  is  very  robust  in  nature.  Since  the  uncertainties  can  be  handled  in  the  control  law 
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by  assuming  a  known  bound  on  the  uncertainty.  In  particular  let  us  consider  a  nonlinear 
state  space  model  of  the  form 


x=f{x,uf)  (2-1) 

where  /  is  not  exactly  known.  The  function  can  be  divided  into  the  sum  of  a  known 
portion  and  an  uncertain  component 

/  (x,u,t)  =  f0  ( x,u,t )  +  a f  (x,u,t)  (2-2) 


where  f0  is  the  known  nominal  function.  A  bound  on  the  uncertainty  a/  is  assumed  to  be 
known  as 

F  (. x,u,t )  s  |  tf  (. x,u,t )  |  (2-3) 


where  F  is  a  known  for  every  x,  u,  and  t.  Although  F  could  be  arbitrarily  large,  an 
extreme  value  of  the  bound  will  result  in  undesirable  chattering  of  the  control  signal.  Tj 
variable  structure  control  technique  is  based  on  the  definition  of  a  sliding  surface,  a(x) 
=  0,  and  switching  law  for  which  the  feedback  control  is  of  the  form 


j  ut(x )  if  o(x)  *  0 
I  ujx)  if  o(x)  <  0 


(2-4) 
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The  switching  law  is  determined  so  that  the  state  is  driven  from  any  initial  condition  x^) 
to  the  sliding  surface  o(x)  =  0  where  it  stays  (ideally,  at  least)  for  all  subsequent  times. 
In  selecting  the  sliding  surface,  the  only  condition  which  must  be  satisfied  is  that  of  stable 
dynamic  response  to  any  set  of  initial  conditions  which  may  be  imposed.  This  can  be 
expressed  mathematically  as  the  following: 

o(x(f))  =  0  for  all  t  >  t0  —  lim  x(t)  =  0  (2-5) 

Based  on  this,  a  possible  choice  of  c(x)  is  a  linear  sliding  surface 

o(x)  =  s  Tx  (2-6) 

where  s  is  a  left  eigenvector  of  A 

sTA  =  -Asr  (2 -?) 

This  definition  combined  with  the  state  space  model 

x  =  Ax  +  bu  +  a/  (2-8) 

yields  the  dynamics  of  o.  Thus,  by  combining  (2-7)  and  (2-8) 

sTx  =  -XsTbu  +  5rV  (2*9) 
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this  expression  is  obtained.  Next  let  o(tj  =  jTx(t),  a  scalar  signal,  and  substitute  this 
expression  into  Equation  (2-9)  to  obtain 

6(0  =  -Ao(0  +  sTbu  +  5  V  (2-10) 


Now  the  control  input  u  can  be  defined  as 

u(0  =  -K(t)  sign(o(t))  (2-11) 

where  K(t)  is  a  time-varying  gain  to  be  chosen  so  that  we  define  the  Lyapunov  function 

V(o)  =  i  (a(t))2  (2-12) 

which  guarantees  that  the  sliding  surface  will  reach  0,  equilibrium,  in  a  finite  amount  of 
time.  This  is  depicted  in  Figure  1  which  illustrates  that,  regardless  of  intial  condition,  the 
state  trajectory  is  driven  to  the  sliding  surface.  This  can  be  found  by  multiplying  Equation 
(2-10)  by  o(t) 

o(6(0)  =  -Ao(0z  +  sTbuo  +  j  Vo  =  V(o)  (2-13) 

which  corresponds  to  the  first  derivative  of  the  Lyapunov  function.  If  we  choose  u  as  in 
(2-11)  and 
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(2-14) 


m  * 


sTF 

'sTb\ 


then 

V(o)  <0  and  V(o)  -  0  (2*15> 

This  in  turn  implies  that  a(x(t))  — »  0  (i.e.,  the  state  approaches  the  sliding  surface  a(x)  = 
0).  In  fact,  by  Equations  (2-10)  and  (2-12)  we  obtain 

K(o)  =  -  (5rh)/T(f)|o|  +  5  Vo  (2*16) 

In  order  for  the  sliding  surface  to  behave  as  in  Figure  1  over  a  finite  interval  of 
time,  the  following  condition  must  be  satisfied: 

6(f)  =  -K(t)2sign(o(t))  (2-17) 

The  condition  of  Equation  (2-17)  is  satisfied  by  choosing  the  control  input  as 

u  +  u  =  u  =  ~{sTbyl stAx  -  K2(sTb)'] sign(o(t))  (2-18) 

Several  important  notes  concerning  the  two  previous  equations  should  be  addressed  at  this 
point.  The  selection  of  AT(t)  should  be  made  only  slightly  larger  than  F,  the  known  portion 
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Figure  1.  Sliding  Surface  Representation 


of  the  uncertainty.  This  is  due  to  the  fact  that,  as  the  gain  K  increases,  more  chatteringwill 
be  introduced  on  the  sliding  plane.  From  Equation  (2-18),  we  note  that  the  control  u  is 
composed  of  two  parts, 


u  =  -{s  Tby's  7 Ax 


(2-19) 


which  is  a  linear  feedback  law,  and 


u  =  -K2(s  Tby'  sign(o) 


(2-20) 


This  part  is  a  nonlinear  feedback  with  its  sign  toggling  between  plus  and  minus  according 
to  which  side  of  the  sliding  plane  the  system  is  located  in.  The  caveat  placed  on  the 
magnitude  of  K  must  also  include  that  it  must  be  large  enough  to  allow  the  system  to 
possess  the  required  degree  of  robustness  to  handle  the  uncertainties  brought  about  by  the 
operating  enviroment  [Ref.  1]. 

C.  COMPUTER  CODING  AND  TESTING  OF  THE  ALGORITHM 

The  first  programs  generated  were  developed  on  the  IBM-PC  using  Matlab  for  code 
generation.  The  real-time  controller  that  was  eventually  realized  was  much  more  complex 
than  this  initial  routine  which  only  tested  the  nonadaptive  sliding  mode  control  derived 
in  the  preceding  section.  The  Matlab  code,  dauv.m,  is  included  in  Appendix  A.  The 
primary  purpose  of  this  code  was  to  test  the  basic  depth  controller  and  obtain  a  set  of 
steady-state  feedback  gains.  These  gains  were  associated  with  the  following  states:  pitch, 
pitch  rate,  and  depth.  The  steady-state  feedback  gains  were  found  using  the  linear- 
quadratic-regulator  function  in  Matlab  which  is  based  on  a  solution  to  the  Riccati 
equation.  These  gains  were  based  on  the  nominal  state  space  model  of  the  SDV,  and,  as 
such,  will  require  adjustment  in  the  future  to  enable  the  AUV  to  perform  as  designed. 

The  state  space  model  used  in  the  depth  control  algorithm  is  based  on  previous 
work  by  Schwartz  [Ref.  3].  This  model  is  discussed  in  detail  in  Chapter  IV.  The  Matlab 
code  is  based  on  the  variable  structure  development  of  the  preceding  section.  The  steady 
state  feedback  gains  are  contained  in  the  variable  L  which  is  a  vector  with  three 
components.  The  program  calls  a  model  of  the  SDV  which  contains  the  dynamic 
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equations  of  motion  developed  by  Boncal  [Ref.  4].  In  order  to  pass  the  updated 
information  determined  by  this  model,  an  array  containing  all  twelve  states  has  been  set 
up.  The  states  of  this  array  which  are  of  interest  to  the  diving  algorithm  are:  state(5)  = 
pitch  rate,  state(9)  =  actual  vehicle  depth,  and  state(ll)  =  pitch.  In  an  effort  to  simplify 
the  code  and  to  reduce  the  error  inherent  with  round  off,  the  depth  error,  "z",  is 
normalized  with  the  absolute  speed.  The  sliding  surface  is  equated  by  multiplying  the  left 
eigenvector  of  the  state  space  model  with  the  error  vector  composed  of  pitch,  pitch  rate, 
and  the  normalized  depth  error.  As  shown  in  the  derivation  of  the  sliding  mode  controller, 
the  input  of  the  dive  command  is  composed  of  a  linear  feedback  component  u  and  a 
nonlinear  feedback  portion  u.  The  maximum  deflection  of  the  dive  planes  is  0.4  radians 
which  is  a  software  factor  and  has  yet  to  be  determined  in  hardware  experimentation. 

1.  Simulation  Results 

The  code  is  simulated  using  iteration  techniques  to  varying  orders  of  depth.  In 
Figure  2  the  graphical  representation  of  the  actual  vehicle  depth  is  shown.  This  graph 
shows  that,  for  a  commanded  depth  of  50  feet  based  on  an  initial  or  reference  depth,  the 
vehicle  has  no  overshoot  and  settles  to  the  desired  depth  change  in  a  minimum  of  time. 
This  simulation  is  based  on  a  vehicle  speed  of  approximately  3  knots  which  is  the 
designed  cruising  speed  of  the  AUV.  Additional  simulations  have  been  run  for  varying 
depth  commands  with  similar  results  (i.e.,  no  overshoot  with  a  minimum  settling  time). 
For  the  dive  depicted  in  Figure  2,  the  corresponding  dive  plane  action  is  shown  in  Figure 
3.  We  note  that  the  action  of  the  dive  plane  is  smooth  and  does  not  exceed  0.4  radians. 
In  direct  correlation  with  the  graph  of  the  vehicle  depth,  we  note  that  the  dive  plane 
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adjusts  to  not  allow  the  vehicle  to  exceed  a  declination  with  respect  to  reference  normal 
of  approximately  0.8  radians.  This  is  accomplished  by  feeding  back  the  pitch  rate  and 
pitch  states.  Figure  4  is  a  depiction  of  the  pitch  rate  which  is  a  direct  reflection  of  the 
divefin  action.  A  measurement  of  the  accuracy  and  smoothness  of  the  vehicle  trajectory 
can  be  assessed  by  graphical  representation  of  the  pitch  rate.  The  pitch  rate  should  possess 
a  nearly  symmetric  shape  with  respect  to  either  side  of  the  mean  value  which  is  zero  for 
constant  trajectories.  This  is,  in  fact,  the  case  of  the  graph  in  Figure  4.  The  last  graph  of 
this  trial  simulation  represents  vehicle  pitch  over  the  run.  When  the  submersible 
approaches  the  commanded  depth,  a  gradual  decrease  in  pitch  should  occur  as  depicted 
in  Figure  5.  This  decrease  in  pitch  should  go  to  zero  for  constant  trajectories  as  is  the 
case.  This  indicates  that  the  vehicle  is  coincident  to  the  reference  axis  of  the  system. 

The  last  graph  of  this  set,  Figure  6,  depicts  the  sliding  surface  behavior.  This 
graph  indicates  that  a  stable  sliding  plane  exist,  and  that,  for  the  given  initial  condition, 
the  state  trajectory  error  will  be  driven  to  zero  along  this  plane.  Many  simulations  have 
been  run  for  varying  conditions  and  depths.  All  of  the  data  collected  indicated  a  stable 
and  highly  accurate  controller  which  was  the  goal  of  this  portion  of  the  project. 
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Figure  2.  Actual  Depth  for  Variable  Structure  Controller  Test 
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Figure  3.  Divefin  Response  to  a  Change  in  Depth  of  50  Feet 
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Figure  4.  Pitch  Rate  Response  to  a  Change  in  Depth  of  50  Feet 


Figure  5.  Pitch  Response  to  a  Change  in  Depth  of  50  Feet 
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Figure  6.  Sliding  Plane  for  Change  in  Depth  of  50  Feet 


D.  ROBUST  OBSERVER  DESIGN 


The  variable  structure  algorithm  developed  in  the  previous  section  is  very  robust  in 
behavior.  The  sliding  mode  control  in  general  can  handle  any  nonlinear  perturbation  due 
to  this  robustness.  This  concept  is  shown  in  Figure  7  where  f,  the  nonlinear  perturbation, 
which  is  possibly  state  dependent  is  shown. 
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Figure  7.  Typical  Sliding  Mode  Scenario 


When  the  state  vector  is  replaced  by  its  estimate,  in  general  a  degradation  in  the 
overall  performance  of  the  system  occurs.  This  is  due  primarily  to  the  fact  that  the 
estimator  introduces  errors  into  the  loop.  As  a  consequence,  a  controller  which  is 
satisfactory  in  a  state  feedback  configuration  might  not  perform  in  a  desirable  way  once 
an  observer  is  introduced.  In  this  context  we  introduce  the  concept  of  robust  observers  in 
order  to  determine  a  class  of  observers  for  which  the  degradation  of  the  closed  loop 
system  performance  is  minimal. 
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1.  Robust  Observer  Derivation  based  on  the  Doyle-Stein  Condition 

It  is  a  standard  result  in  system  theory  that  in  ideal  situations  the  observer 
dynamics  do  not  affect  the  dynamics  of  the  closed-loop  system.  This  can  be  seen  easily 
by  considering  the  equations  of  the  closed-loop  system 


x  =  Ax  +  Bu 
y  =  Cx 
u  =  -Lx  +  r 


—  =  Ax  +  Bu  +  K(y  -  Cx) 

dt 


(2-21) 


and  deriving  the  transfer  function  (in  the  SISO  case)  Y(s)/R(s).  However,  this  does  not 
mean  that  the  observer  docs  not  have  any  impact  in  the  dynamics  of  the  overall  system. 
In  particular,  we  can  see  that  in  the  case  of  a  disturbance  entering  the  input,  for  example, 
as 


x  -  Ax  *  b(u  +  /) 
y  -  Cx 
u  -  -Lx  +  r 

£  «  Ax  *  Bu  *  K(y-Cx) 

dt 


(2-22) 


with  /  the  disturbance  term.  The  transfer  function  relating  /  to  y  is  affected  by  the 
observer,  and  it  could  cause  either  instability  of  the  system  or  poor  characteristics  in 
terms  of  phase  and  gain  margins. 

However,  when  we  know  the  precise  point  of  entry  of  the  disturbance  /  as  in 
Equation  (2-22),  we  can  determine  the  observer  gain  K  so  to  minimize  its  effect  on  the 
system  performance.  It  has  been  shown  in  Ref.  10  that  in  the  limiting  case  of  K  such  that 
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(2-23) 


K[I  +  C(sl  -  AY'KY'  =  B[C(sI  -  A)'1*]-' 

the  observer  dynamics  do  not  appear  in  the  transfer  function  between  /  and  y.  As  a 
consequence,  the  system  (2-22)  and  its  state  feedback  equivalent 

x  =  Ax  +  B(u  +  /) 

y  =  Cx  (2-24) 

u  =  -Lx  +  r 


behave  in  the  same  manner.  Condition  (2-23)  is  called  the  Doyle-Stein  condition  and  the 
resulting  observer  is  referred  to  as  a  robust  observer. 

It  turns  out  that  the  Doyle-Stein  condition  (2-23)  can  be  satisfied  only  as  a  limiting 
case.  In  other  words  it  has  been  shown  in  Ref.  10  that  a  sequence  of  observer  gains  K(Q), 
with 

K(Q)  =  P(Q)ctRw'  (2-25) 

where  is  an  arbitary  definite  matrix  and  P(Q)  is  the  solution  of  the  Riccati  equation 
AP(Q )  +  P(Q)AT  +  q2BBT  -  P(Q)c  TRw'cP(Q)  =  0  (2-26) 

with  q  a  scalar,  positive  parameter,  is  such  that 
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(2-27) 


lira  K(Q)  =  K 

q  -  +m 

with  K  satisfying  the  Doyle-Stein  condition  (2-23). 

The  intuitive  idea  behind  this  agniment  is  to  design  an  observer  "as  if'  the 
disturbance  /  (not  necessarily  white  or  even  random)  were  white  noise  with  covariance 
q2.  This  leads  to  a  steady-state  Kalman  filter  design,  Equations  (2-25)  and  (2-26),  where 
the  degree  of  robustness  of  the  observer  increases  as  q  increases  [Ref.  5],  Therefore,  we 
should  be  able  to  determine  the  optimal  value  of  q  using  the  Nyquist  criterion  and  the 
continuous-time  Nyquist  plot.  This  plot  should  show  a  point  of  convergence  for  the 
maximum  phase  and  gain  margins  capable  of  being  achieved  by  our  dive  system.  This 
point  of  convergence  should  give  a  value  for  q  such  that  the  dynamics  of  the  observer  do 
not  effect  the  closed-loop  characteristics  of  the  system.  Figure  8  is  the  continuous -time 
Nyquist  plot  for  the  dive  system.  We  note  that,  as  q  increases  in  magnitude,  the  Nyquist 
plot  converges  to  a  optimal  value.  This  value  theoretically  should  give  a  optimum  time 
response  for  depth  and  dive  plane  action.  Figure  9  is  a  depiction  of  these  time  response 
plots  for  the  value  of  q  =  200  which  was  the  point  of  convergence  on  the  Nyquist  plot. 
It  is  t  /idem  from  Figure  9  that  the  optimal  value  for  the  parameter  "q"  is  not  200  as 
suggested  by  the  continuous-time  Nyquist  plot  and  supported  by  the  current 
documentation. 
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The  dive  plane  response  plot  of  Figure  9  suggested  that  q  =  200  was  too  large 
in  magnitude  due  to  the  chattering  of  the  dive  command.  This  led  to  a  hueristic  approach 
to  determine  the  appropriate  value  for  q.  The  optimal  value  for  the  dive  system  was  found 
to  be  5.  The  problem  was  how  to  justify  this  value  mathematically  and  to  prove  the 
existence  of  a  stability  window  that  occurs  when  discrete  controllers  are  used. 

a.  Graphical  Evidence  of  a  Stability  Window  in  Discrete-Time 

To  study  the  behavior  of  the  closed-loop  system  for  different  values  of 
the  parameter  q,  the  discrete-time  Nyquist  plot  was  used.  In  discrete-time  the  Nyquist 
stability  criterion  is  equivalent  to  the  one  in  continuous-time,  and  the  closed-loop  behavior 
is  inferred  from  phase  and  gain  margins.  Although  we  have  shown  in  the  previous  section 
that  the  use  of  a  robust  observer  improves  the  loop  characteristics,  in  the  actual 
implementation  we  have  to  exercise  some  care  in  the  choice  of  the  parameter  q.  This  is 
due  to  the  fact  that  we  have  assumed  that  the  disturbance /in  Equation  (2-22)  enters  from 
the  input  only.  If  other  sources  of  disturbances  are  present  (i.e.,  entering  at  the  output), 
they  can  introduce  instabilities  if  the  value  of  the  parameter  q  in  Equation  (2-26)  is  too 
large.  In  order  to  verify  this,  let  us  break  the  loop  at  the  input  u  and  consider  the  transfer 
function  u^  -»  u,  which  is  shown  in  the  block  diagram  of  Figure  10.  This  transfer  function 
is  derived  in  the  following  manner: 

x  -  Ax  +  Bu2  (2-28) 
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Figure  10.  Block  Diagram  Showing  System  Broken  at  Input  "X"  and  Output  ’’XX" 
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B 

(2-35) 

x.  + 

„  u. 

KC 

( A-KC-LB )J 

QJ  2 

=  [Q  L]xt  , 

(2-36) 

the  transfer  function  of  Uj  — >  u,  in  state-space  form.  We  can  compute  the  frequency 
response  of  the  loop  in  Figure  10  open  at  the  point  marked  with  "X".  Using  this  transfer 
function,  a  discrete  Nyquist  plot  was  obtained  for  various  values  of  q:  5,  50,  150,  and 
200.  This  plot  is  shown  in  Figure  1 1 .  From  a  comparison  of  this  plot  and  that  of  Figure 
8,  we  see  that  the  discrete  time  transformation  effects  caused  no  noticeable  deviations  in 
the  frequency  response  for  all  values  of  the  parameter  q. 

As  mentioned  above,  the  only  perturbation  on  the  system  taken  into 
account  so  far  is /which  enters  at  the  input  as  depicted  in  Figure  7,  but  in  fact  the  system 
is  also  corrupted  by  noise  at  the  output.  If  we  want  to  see  the  effect  of  a  disturbance  at 
the  output  as  q  changes,  we  have  to  open  the  loop  at  the  point  marked  "XX"  in  Figure 
10  and  study  this  closed-loop  transfer  function  y2  — »  y,.  It  can  be  shown  that  the  state- 
space  representation  of  the  dynamics  y2  — >  y,  is  given  by 

A  -LB  IK' 

X‘  =  Q  A-KC-LB\  X‘  +  |_Q j  h  (2-37) 

>’,  =  [C  0]xe 
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The  Nyquist  plot  of  the  corresponding  frequency  response  depicted  in 
Figure  12  shows  some  of  the  problems  encountered  for  large  values  of  q.  We  note  that, 
while  the  phase  margin  and  gain  margin  increase  in  Figure  11,  a  corresponding  decrease 
is  occurring  in  the  phase  margin  of  Figure  12  for  an  associated  increase  in  the  parameter 
q.  Therefore,  the  noise  present  at  the  output  causes  a  reverse  effect  on  the  system  for 
increasing  values  of  q.  The  effect  of  this  is  chattering  in  the  dive  plane  response  which 
occurs  for  large  values  of  q.  As  the  gain  and  phase  margins  increase  at  the  input,  the 
phase  margin  is  diminishing  at  the  output  of  the  system. 

We  can  see  from  the  Nyquist  plots  in  Figure  12  that  as  q  increases  the 
gain  margin  decreases.  In  particular,  from  the  phase  margin,  we  can  see  that  the  closed- 
loop  response  of  the  system  seen  from  a  disturbance  entering  at  the  output  becomes  more 
and  more  under-damped  for  larger  values  of  q.  Experimental  results  show  that  a  value  of 
q  =  5  yields  a  reasonable  closed-loop  response.  This  can  be  seen  by  comparing  the  step- 
response  plots  of  the  robust  observer  depicted  in  Figure  1 3  for  the  value  of  q  =  5  with 
those  of  q  =  200  in  Figure  9. 

The  continuous -time  behavior  of  the  Doyle-Stein  condition  appears  to  not 
account  for  the  noise  at  the  output  in  a  similar  fashion  to  that  of  the  discrete-time. 
However,  further  investigation  of  this  topic  is  warranted.  Graphical  evidence  does  support 
the  adverse  effect  of  a  nonlinear  perturbation  present  at  the  output  in  discrete-time  control 
for  submersible  vehicles  such  as  the  Naval  Postgraduate  School  Autonomous  Underwater 
Vehicle. 


27 


29 


par  am e r 


E.  SENSOR  ERROR  DETECTION  AND  HANDLING  EM  SOFTWARE 

The  purpose  of  the  robust  observer  for  the  AUV,  which  is  depicted  in  block 
diagram  form  in  Figure  14,  is  to  provide  redundancy  in  systems  through  software  vice 
hardware.  The  observer  developed  in  the  previous  section  provides  a  certain  degree  of 
redundancy  in  the  control  of  the  AUV.  In  fact,  for  every  signal  in  the  loop  (pitch,  pitch 
rate,  and  depth  in  our  case),  both  the  measured  and  estimated  signals  are  available. 

Based  on  this  we  can  determine  an  error  detection  algorithm  which  alerts  the  system 
in  case  of  sensor  failure  and  replaces  the  faulty  values  by  the  estimates.  This  offers  an 
advantage  in  terms  of  reliability,  and  it  allows  for  a  stable  control  even  in  the  presence 
of  one  or  more  sensor  failures.  The  principle  of  this  error  detection  scheme  is  simple. 
Through  the  use  of  a  fast  computer  algorithm,  it  checks  error  thresholds  between 
measured  state  and  observer  estimated  state.  Then  the  algorithm  selects  the  most  accurate 
state  representation  and  uses  it  for  control.  By  this  we  mean  that,  if  an  error  was  detected 
in  the  sensor  output,  the  estimated  state  would  be  used  instead  of  the  faulty  measured 
state. 

1.  Development  of  the  Error  Detection  Algorithm 

The  error  between  actual  state,  x,  and  estimated  state,  A,  is  defined  as 

x  =  x  -  x  (2-38) 
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Figure  14.  Block  Diagram  of  Robust  Observer 


32 


The  measured  state  is  obtained  in  the  AUV  prototype  directly  from  the  sensors  (i.e., 
gyros,  sonars,  and  servo-controllers).  Each  sensor  sends  the  information  to  a  signal 
conditioning  network  and  then  to  a  designated  area  in  memory  where  the  autopilot 
samples  the  data.  In  order  for  the  error  detection  concept  to  be  realized,  the  typical  error 
signal,  x  ,  should  be  as  small  as  possible  during  normal  operations,  at  least 
asymptotically;  however,  transient  responses  due  to  the  observer  dynamics  introduce 
errors  even  in  ideal  conditions.  A  graphical  representaion  of  the  pitch  rate  and  pitch  error 
signals  in  Figure  15  shows  that  f  in  the  transient  phase  is  far  from  zero.  These 
transients  are  present  due  to  the  initial  conditions  placed  on  the  observer.  In  this  section 
we  determine  an  error  criterion  which  is  not  affected  by  the  transient  response  of  the 
observer.  Our  arguments  are  an  application  of  the  Cayley-Hamilton  theorem. 

In  the  ideal  case  the  error  between  actual  and  estimated  state  satisfies  the  state 
space  equation 


x(k  *  1)  =  90x(k) 


(2-39) 


with  O0  the  discrete  transition  matrix  of  the  observer,  of  the  form 

Q0  =  9  -  KC  (2-40) 
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In  our  particular  case,  the  state  error  x  is  given  by 


Q 

X  = 

e 

- 

* 2 

z 

*3. 

V  _ 

(2-41) 


where  v  is  vehicle  forward  speed.  Since  the  observer  gain  K  is  chosen  to  guarantee 
exponential  stability  of  Equation  (2-39),  we  can  see  that  x{k)  has  an  effect  only  during 
the  transient  response.  In  order  to  determine  an  error  criterion  which  is  not  affected  by 
the  transient  response  and  is  zero  (or  very  small)  during  normal  operations,  consider  the 
Cayley-Hamilton  theorem  applied  to  the  matrix  <D0, 

V  +  +  •••  +  anI  =  0  (2-42) 


where 


A(X)  =  A"  -  a{\n  '  +  a2\n  l  *  -  -  an  =  det(A7  -  $„)  (2-43) 

is  the  characteristic  polynomial  of  <I>0.  Application  of  Equation  (2-42)  to  Equation  (2-39) 
yields,  at  least  under  ideal  conditions, 

e{k)  =  x(k)  +  ajX(A-l)  +  ■■  +anx(k-ri)  =  0  (2-44) 
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for  all  k,  since 


x(k)  *  axx(k-\ )  +  -  +  anx(k-n )  =  ^ 

(®0n  ax 4»0B  l  +  -  +  anI)x(k~n ) 

which  is  zero  by  the  Cayley-Hamilton  theorem. 

2.  Testing  of  the  Error  Detection  Algorithm 

The  error  detection  algorithm  was  tested  by  simulating  a  sensor  failure.  This 
was  done  in  the  code  auvobs.m  found  in  Appendix  A.  To  simulate  a  sensor  failure,  the 
sensor  in  question  is  driven  to  zero  to  indicate  a  broken  connection  and  to  100,000  to 
indicate  the  program  accessing  a  null  pointer  region.  Both  of  these  error  conditions  reflect 
possible  error  sources  which  can  actually  occur.  To  notify  the  program  developer  and  user 
that  the  system  had  detected  an  error,  "C"  language  standard  output  printf  statements  were 
used.  The  actual  real-time  code  documents  error  conditions  differently  by  setting  flags  in 
a  file  that  contains  the  vehicle  run  data.  Also  a  faulty  sensor  or  broken  connection  will 
probably  not  result  in  a  polled  value  of  0.0,  but  for  simulation  purposes  this  value  was 
used  to  show  the  sensitivity  of  the  error  detection  algorithm. 

The  algorithm  takes  the  corrected  error  vector  and  compares  the  values  with 
the  threshold  levels  previously  mentioned.  If  the  corrected  errors  are  within  the  tolerance 
setup  by  the  threshold  values,  then  the  actual  state  is  used,  else  the  observed  state  is  used 
and  an  error  is  assumed  to  have  occurred.  Several  runs  depicting  various  error  conditions 
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Depth  Change  of  50  Feet 


are  shown  in  Figures  17,  18,  and  19.  These  figures  only  show  the  magnitudes  present 
under  these  error  conditions.  In  Figure  17  theerror  simulated  is  a  broken  connection  to  the 
pitch  rate  gyro.  When  the  plots  of  Figure  17  are  compared  to  those  of  Figure  16,  we  note 
the  large  magnitude  of  error  present  at  the  sensor  failure  point.  This  error  diminishes 
rapidly,  and,  even  though  a  sensor  error  is  still  present,  the  errors  are  below  the  threshold. 
Therefore,  the  autopilot  does  not  need  to  discriminate  between  the  observer  state  and  the 
actual  state. 

This  error  diminishes  rapidily  because  the  controller  drives  the  error  to  zero 
in  steady  state  and  thus  the  effects  of  this  type  of  error  (i.e.,  a  returned  value  of  0.0  from 
the  failed  sensor)  are  negligible.  However,  in  Figure  18,  a  null  pointer  assignment  error 
is  simulated.  This  type  of  error  occurs  when  a  program,  such  as  the  sensor  polling 
program  for  the  AUV,  accesses  a  region  of  memory  which  is  marked  for  i.ull 
assignments.  When  this  occurs,  the  value  returned  can  take  on  a  spurious  assignment  such 
as  the  one  modeled  in  the  simulation.  From  these  plots  we  note  that,  regardless  of  the 
magnitude  of  the  error,  the  other  sensors  are  not  effected.  This  is  due  to  the  fact  that  the 
sensors  are  independent  of  each  other  in  hardware  and  software.  The  final  set  of  plots 
shown  in  Figure  19  demonstrate  a  pitch  sensor  failure.  This  plot  along  with  the  others 
shows  that,  when  a  sensor  failure  occurs,  the  resultant  error  exceeds  the  threshold  level 
and  causes  the  observer  state  to  be  used.  Once  again  the  failure  value  of  0.0  was  used  to 
demonstrate  the  sensitivity  of  the  error  detection  method.  While  the  pitch  sensor  failed, 
we  note  again  that  the  pitchra te  sensor  is  not  affected,  and  the  error  does  not  exceed  the 
threshold  level. 
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Threshold  Levels  for  Sensor  Failure  Detection 


Figure  18.  Plots  of  Pitch  and  Pitch  Rate  with  Threshold  Levels  for  Sensor  Failure  Detection 


t  a 


Figure  19.  Plots  of  Pitch  and  Pitch  Rate  Errors  with  Threshold  Levels  for  Sensor  Failure  Detection 


3.  Fatal  Sensor  Errors 


Most  sensors  can  be  modeled  with  sufficient  accuracy  to  be  used  in  place  of 
the  actual  state,  but  some  errors,  if  detected,  are  considered  major  and  cause  mission 
termination  upon  detection.  Three  such  errors  are  depth  sensor  error,  directional  gyro 
error,  and  erroneous  input  to  the  autopilot  from  the  mission  planner.  All  of  these 
conditions  are  closely  monitored,  and  if  any  one  of  these  conditions  is  detected  the  AUV 
programmed  run  is  terminated.  The  AUV  is  forced  to  surface  and  slows  to  the  minimum 
required  speed  necessary  for  the  vehicle  to  remain  on  the  surface.  The  rudder  is  also  set 
to  zero  deflection  when  a  fatal  error  occurs. 

Depth  sensor  gyro  failures  are  detected  in  the  same  manner  as  those  of  the 
pitch  and  pitchrate  gyros.  A  threshold  level  is  compared  to  the  corrected  error  returned 
by  the  difference  of  the  estimated  and  actual  states.  This  concept  is  depicted  in  block 
diagram  form  in  Figure  20.  This  diagram  demonstrates  how  all  of  the  components  are 
linked  in  the  algorithm. 

The  error  detection  method  used  for  sensing  mission  planner  errors  and 
directional  gyro  failure  is  different  from  the  previous  scheme.  This  algorithm  does  a  check 
of  the  incoming  values  and  compares  these  values  to  2k.  If  the  incoming  value  is  >  2k. 
then  a  sensor  error  has  occurred.  The  vehicle  is  forced  to  surface  and  the  rudder  is  set  to 
0  deflection.  This  terminates  the  AUV  mission  and  protects  the  prototype  vehicle  from 
being  damaged  due  to  sensor  failure  or  incorrect  desired-headings  generated  by  the 
mission  planner. 
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ERROR  DETECTION  CONCEPT 


F.  SIMULATION  OF  REAL-TIME  AUTOPILOT 

With  the  development  of  the  robust  observer  and  the  error  detection  algorithm,  the 
autopilot  routine  is  complete  and  ready  for  simulation  and  implementation.  The  final 
check  of  the  proposed  autopilot  was  conducted  simulating  the  "C"  source  code  using  the 
programs  and  interface  module  discussed  in  Chapter  3.  The  Matlab  simulation  code, 
auvobs.m,  is  located  in  Appendix  A  while  the  "C"  source  code,  autopilot.c,  is  located  in 
Appendix  B. 

During  this  final  evaluation  period,  a  multiple  dive  simulation  was  run.  This 
simulation  detected  a  fault  in  the  error  detection  algorithm.  This  fault  occurred  because 
the  error  vectors  must  go  through  two  iterations  of  the  control  routine  after  a  new  ordered 
depth  is  received.  This  is  due  to  the  fact  that  the  error  vectors  operate  on  previous  states 
as  shown  in  Equation  (2-24).  To  correct  this  fault,  a  flag  is  used  which  sets  the  variable 
"transient_flag"  to  a  value  such  that  the  error  vectors  are  allowed  to  update  prior  to  being 
operated  on  by  the  coefficents  of  the  characteristic  equation  of  the  A  matrix. 

In  the  C  code  version  of  the  autopilot,  the  current  ordered  depth  is  compared  to  the 
previous  ordered  depth.  If  the  values  are  identical  then  error  checking  continues,  but, 
when  these  values  are  different,  a  flag  "transient_hold"  is  set  using  a  discrete  counter 
which  increments  each  time  the  autopilot  routine  is  called.  This  allows  the  error  detection 
algorithm  to  operate  in  a  discrete  mode  as  intended.  The  transient  flag  can  only  be  re¬ 
initialized  when  a  change  in  ordered  depth  has  been  sent  to  the  controller. 
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A  multiple  depth  change  simulation  is  depicted  in  Figures  21-24.  This  simulation 
was  based  on  the  following  initial  conditions  and  simulated  errors: 

•  Initial  Depth:  100  Feet. 

•  Initial  Velocity:  4  Knots. 

•  Initial  Propeller  Revolution  Rate:  500  RPM. 

•  Parameter  q  set  to  5. 

•  First  Ordered  Depth:  140  Feet. 

•  Second  Ordered  Depth:  120  Feet  (Begins  at  Frame  226). 

•  Duration  of  Simulation  Run:  400  Frames. 

•  Simulated  Error  in  Pitch  Rate  Gyro  (Pitch  Rate  Sensor  =  100000). 

•  Simulated  Failure  of  Pitch  Gyro  (Pitch  Sensor  =  -35). 

•  Pitch  Rate  Threshold  Error  set  to  0.005. 

•  Pitch  Threshold  Error  set  to  0.05. 

•  Normalized  Depth  Error  set  to  0.2. 

The  simulation  has  two  induced  sensor  errors.  The  first  error  occurs  at  time-frame  5  when 
the  value  of  100000  is  returned  during  polling  of  the  pitch  rate  gyro.  The  second  error 
occurs  at  time  frame  50  when  the  value  of  -35  is  returned  during  sampling  of  the  pitch 
gyro.  The  pitch  rate  gyro  error  is  an  example  of  a  null  pointer  assignment  while  the  pitch 
gyro  error  is  a  general  sensor  failure.  As  noted  before,  these  are  both  plausible  values  for 
their  associated  errors.  In  Figure  21  plots  of  the  corrected  pitch  and  pitch  rate  errors  with 
their  associated  thresholds  are  shown.  From  these  plots  we  note  the  beginning  of  the 
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sensor  failure  by  the  jump  in  the  error.  Both  errors  cause  the  direct  sampled  states  to  not 
be  used  and  instead  the  estimated  states  returned  by  the  observer  are  used.  The  noticeable 
drop  in  magnitude  of  these  two  errors  that  occurs  at  frame  226  is  due  to  the  change  in 
ordered  depth  which  calls  for  resetting  the  error  vectors.  The  hiatus  required  to  reset  the 
error  vectors  causes  the  magnitude  of  both  errors  to  momentarily  fall,  but,  when  the  error 
detection  algorithm  is  reset,  the  errors  rise  and  again  cause  the  observed  state  to  be  used 
vice  the  actual  measured  state  in  the  controller.  The  time  in  seconds  of  this  hiatus  is 
approximately  0.1  seconds,  which  even  for  a  slow  sampling  system  such  as  the  AUV,  is 
negligible.  The  next  set  of  plots  shown  in  Figure  22  are  of  the  normalized-depth  error  and 
actual  pitch  rate  of  the  vehicle.  The  plot  of  normalized-depth  error  demonstrates  that  the 
errors  induced  in  the  pitch  and  pitch  rate  gyros  have  no  effect  on  it.  From  previous 
simulations  this  is  what  we  expect.  The  depth  error  does  not  exceed  the  threshold,  and, 
therefore,  a  fatal  sensor  error  is  not  detected.  The  plot  of  the  pitch  rate  of  the  vehicle  is 
a  good  indicator  of  the  system  stability.  The  vehicle  pitch  rate  curve  should  be  symmetric 
about  the  mean  which  is  0  in  steady  state.  This  indicates  a  stable  platform  and  is,  in  fact, 
the  type  of  curve  shown  in  Figure  22.  Figure  23  depicts  plots  of  the  actual  depth  with  the 
ordered  depth  overlain  and  the  stem  plane  or  dive  fin  response.  This  set  of  plots  shows 
that  the  controller,  even  with  two  sensor  failures,  is  capable  of  driving  the  vehicle  to  the 
desired  depth  with  no  overshoot  and  a  very  smooth  motion.  The  dive  plane  response  is 
nearly  optimal  and  also  does  not  show  any  influence  of  the  sensor  errors  on  its  control. 
This  demonstrates  the  robust  nature  of  the  controller-observer  combination.  The  last  plot 
contained  in  Figure  24  is  the  final  verification  of  system  stability.  This  plot  is  of  the 
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sliding  plane.  As  mentioned  earlier  in  this  chapter  and  depicted  in  Figure  1,  the  controller 
should  drive  the  state  trajectories  to  the  equilibrium  point  along  this  switching  plane.  This 
is  the  case  shown  Figure  24.  From  the  sliding  plane,  we  can  deduce  that  two  errors 
occurred  during  the  run.  The  change  in  ordered  depth  also  caused  the  states  to  be  offset 
at  time-frame  226.  The  final  result  of  the  sliding  plane  was  the  states  were  in  fact  driven 
to  equilibrium.  This  is  the  last  necessary  component  needed  to  prove  stability  in  the 
variable  structure  controller  design. 
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Figure  24.  Plot  of  the  Sliding  Plane 


IH.  CONSIDERATIONS  FOR  REAL-TIME-IMPLEMENTATION  OF  THE 

AUV  CONTROLLER 

A.  DEVELOPMENT  OF  A  PC-BASED  AUV  MODEL 

•  •  •  - . 

To  facilitate  development  and  testing  of  the  various  programs  to  be  implemented 
on  the  prototype-vehicle,  a  computer  model  of  the  vehicle  dynamics  has  been  developed. 
This  model  presents  an  interface  that  is  similar  in  nature  to  the  actual  hardware 
implementation.  By  this  we  mean  that  the  sampling  of  the  actual  measured  states  on  the 
prototype  by  the  control  programs  should  be  similar  to  the  methods  used  in  simulation. 
A  model  for  the  purposes  of  simulation  was  previously  developed  by  Schwartz  [Ref.  3] 
which  described  the  dynamics  of  a  17.4  feet  long  vehicle,  weighing  12000  pounds,  and 
having  neutral  buoyancy.  This  previous  work  was  taken  as  the  basis  of  the  real-time 
model  developed  here. 

The  model  has  six  degrees  of  freedom:  three  referring  to  position,  the  Euler  angles, 
and  three  to  rotation.  Figure  25  shows  the  right-hand  coordinate  system  of  the  vehicle 
which  the  control  code  is  based  on,  as  well  as  an  artist’s  rendition  of  the  AUV.  To  define 
the  coordinate  system  setup  by  these  angles,  the  vector  of  orientation  and  rate  of  change 
had  to  be  taken  in  to  account.  In  order  to  describe  the  right-hand  coordinate  system  for 
computer  computations,  the  model  consists  of  the  12  states  listed  in  Table  1,  where  the 
mathematical  symbol,  state  definition,  and  computer  program  variable  are  annotated. 

In  Appendix  C  the  program,  model. c,  and  header  file,  mo  '.Iprm.h,  which  contains 
the  parameters  of  the  AUV  have  been  listed.  This  version  of  the  model  features  the  states 
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in  array  format  which  allows  the  calling  routines  to  access  the  model  by  passing  a  pointer 
to  the  first  address  of  the  array.  This  is  a  significant  improvement  over  the  previous 
version  of  the  model  which  used  all  gobal  variables,  a  method  that  has  been  generally 
shunned  by  programmers.  Due  to  the  capabilities  of  the  "C"  language,  the  visibility  of 
functions  and  variables  can  be  easily  modified  [Ref.  6].  (A  variable  with  global  visibility 
can  be  accessed  by  any  function  and  modified,  where  as  a  variable  which  is  locally 
visible  to  its  parent  function  only  cannot  be  modified  by  any  other  function.) 

The  actual  AUV  implementation  will  use  these  visiblity  rules  in  a  similar  manner 
to  that  of  the  integrated  test  package  which  uses  the  model  to  update  the  states  for 
simulation  purposes.  This  updated  model  also  corrected  several  serious  coding  hazards 
such  as  the  initialization  of  an  array  beginning  with  an  index  of  1  vice  0,  which  led  to  an 
extraneous  use  of  zeros  for  array  padding.  This  critical  mistake  can  cause  null  pointer 
assignments  which  result  in  run-time  errors  if  a  program  references  the  array  outside  of 
its  bounds.  Another  notable  improvement  which  lends  the  model  more  to  real-time- 
simulation  code  development  was  the  update  method  used  for  the  control  surfaces,  rudder, 
stemplane,  bowplane,  and  propeller  revolution  rate.  These  values  are  passed  in  a  similar 
manner  to  the  states  (i.e.,  using  pointers). 

The  model  calls  one  external  function,  signum.c,  which  accepts  a  variable  that  has 
been  declared  as  a  double,  and  returns  a  double  which  has  the  value  of  +1.0  or 
-1.0  depending  on  the  sign  of  the  passed  argument.  The  function  modelO  passes  the 
following  arguments:  a  pointer  to  the  address  of  the  first  value  of  oldstate,  a  pointer  to 
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the  address  of  the  first  value  of  inputs,  and  the  sampling  interval.  It  returns  the  address 
of  the  first  value  of  the  updated  state  array  and  a  integer  which  can  been  used  to 
determine  if  an  error  has  occurred  during  the  state  update. 


Table  1.  STATE  DEFINITIONS  FOR  AUV  MODEL 


Mathematical 

Symbol 

State 

Definition 

Program 

Variable 

x-y-z-axis 

Definition 

u 

surge  rate 

mstate[0] 

x-axis 

velocity 

V 

sway  rate 

mstate[l] 

w 

heave  rate 

mstate[2] 

z-axis 

velocity 

P 

roll  rate 

mstate[3] 

rotation  rate 
about  x-axis 

q 

pitch  rate 

mstate[4] 

rotation  rate 
about  y-axis 

r 

yaw  rate 

mstate[5] 

rotation  rate 
about  z-axis 

X 

surge 

mstate[6] 

x-axis 

y 

sway 

mstate[7] 

y-axis 

z 

heave 

mstate[8] 

z-axis 

<}> 

roll 

mstate[9] 

rotation 
about  x-axis 

e 

pitch 

mstateflO] 

rotation 
about  y-axis 

¥ 

yaw 

mstate[ll] 

r  tation 
about  z-axis 

stuay 


Figure  25.  Sketch  of  the  AUV  with  Euler  Angles  "From  Ref.  5. 


B.  INTEGRATED  SIMULATION  ENVIRONMENT  INTERFACE  PROGRAM 
The  control  code,  developed  in  chapter  2,  and  the  model  simulation  code  required 

an  interface  program  that  would  mock  the  actual  hardware  integration  with  the  software. 
The  program,  auvs.c,  performs  this  vital  interface  function,  as  well  as  demonstrating  how 
field  evaluation  methods  and  troubleshooting  with  AUV  run  data  can  be  accomplished. 
It  features  a  scheme  for  calling  the  functions  which  make  up  the  real-time  software  and 
the  simulation  code.  It  also  gives  one  method  for  storing  vehicle  run  data.  This  was  the 
method  used  in  the  testing  and  simulation  of  the  autopilot  program  which  used  the  Doyle- 
Stein  Observer.  The  suggested  method  for  the  actual  AUV  run  data  storage  would  be  to 
open  a  data  file  upon  completion  of  the  autopilot  update.  This  would  allow  the  current 
position  data  and  control  surface  actions  to  be  congruent;  however,  this  is  not  the  order 
found  in  the  interface  program  because  the  system,  like  all  simulations,  has  a  set  of  initial 
conditions  which  perturb  the  control  system  to  perform  a  set  of  actions.  This  was  the  only 
simulation  difficulty  which  could  not  be  accounted  for  in  the  integrated  package. 

C.  FIELD  EVALUATION  GRAPHICS  ROUTINE 

The  data  processed  by  the  AUV  during  an  actual  deployment  may  have  the  need 
for  immediate  graphical  interpretation.  This  case  could  arise  due  to  a  malfunctioning 
program  or  sensor.  It  was  therefore  essential  to  write  a  graphics  routine  which  would  take 
the  uninterpreted  data  and  graphically  depict  it  with  little  effort  by  the  evaluator. 


56 


1.  C  Functions  used  to  Implement  the  Graphics  Routine 

The  main  program,  plot.c,  calls  numerous  functions  to  perform  various  tasks. 
The  majority  of  these  functions  are  system  calls  which  are  specific  to  Microsoft  C  version 
5.1.  Therefore,  it  is  imperative  that  any  future  modifications  made  to  the  files  composing 
the  graphics  routine  be  compiled  using  the  Microsoft  C  5.1  compiler.  To  invoke  the 
executable  file,  type  graph  <filename>. 

The  data  file  should  be  set  up  in  the  same  format  as  that  produced  by  the 
interface  program.  In  particular,  the  file  should  consist  of  column  headings  followed  by 
the  output  of  a  particular  state,  sensor,  or  control  surface  in  column  format.  The  first 
column  of  the  data  file  will  always  be  taken  as  the  abscissa.  Therefore,  to  ensure  that  the 
graphs  represent  meaningful  data,  time  should  always  be  the  first  column  of  the  data  file. 
The  ordinate  label  is  the  current  heading  of  the  data  file.  The  source  code  for  all  of  the 
graphics  files  are  in  Appendix  D. 

a.  Main  Program  PLOT 

This  file  sets  up  the  graphics  environment,  as  well  as  actually  creating  the 
graphs.  Numerous  system  functions  once  again  are  used;  the  prototypes  and  descriptions 
of  these  functions  can  be  found  in  Microsoft  C  version  5.1  documentation.  The  main 
program  calls  one  user-defined  function  which  is  graphics_mode.  The  next  section  deals 
with  this  function  in  detail.  A  header  file  is  also  associated  with  the  graphics  files,  grph.h. 
This  file  contains  structures  and  prototypes  common  to  both  of  the  primary  files. 
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The  program  begins  by  setting  up  buffers  for  the  storage  of  data  file 
headings,  data,  graph  titles,  and  graph  subtitles.  The  defaults  for  plot  representation  are 
white  foreground,  black  background,  and  solid  line  representing  the  plotted  data.  The  user 
is  prompted  for  the  use  of  grids  on  the  graph.  The  only  correct  replies  to  this  prompt  are 
lowercase  y  or  n;  any  other  response  will  cause  the  prompt  to  reappear.  Due  to  the  nature 
of  the  program,  which  is  to  plot  dynamic  arrays,  the  program  must  perform  some  learning 
tasks.  Therefore,  it  is  not  as  rapid  a  plotting  routine  as  one  that  knows  the  size  of  the 
array  prior  to  compilation  time,  but  the  capability  to  plot  varying  array  sizes  is  a 
requirement  for  field  evaluation.  The  program  determines  the  array  size  by  reading  the 
data  file  the  first  time  through.  During  this  first  read,  the  program  determines  how  many 
columns  to  expect  and  the  length  of  the  columns  or  number  of  rows  in  the  array.  The 
program  does  not  store  or  act  on  any  of  the  actual  data  until  the  second  read. 

The  capability  of  the  program  to  dynamically  set  the  data  storage  area  is 
a  feature  which  is  a  strong  point  of  C  [Ref.  6].  The  plots  are  displayed  in  succession  (i.e., 
order  is  determined  by  the  column  in  the  data  file),  and  are  held  on  the  screen  until  the 
user  presses  the  return  key.  The  graphs  are  autoscaled,  no  manual  scaling  is  available  to 
the  user,  so  that  the  largest  and  smallest  values  are  depicted.  A  typical  output  of  the 
program  is  represented  by  Figure  26.  Prior  to  terminating,  the  executable  code  resets  the 
video  mode  of  the  computer  system  back  to  its  previous  mode. 
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b.  Function  GRAPHICS  MODE 

This  function  is  contained  in  the  file,  setvid.c,  and  is  called  by  the 
function,  plot.c.  It  does  not  pass  any  arguments  and  returns  a  void.  The  function  checks 
the  computer  system  for  a  graphics  card.  If  a  graphics  card  is  present,  the  card  is 
automatically  placed  in  its  highest  resolution  mode.  This  will  cause  a  problem  if  the 
system  that  the  program  is  running  on  has  a  Video  Graphics  Adapter  card  (VGA)  which 
is  set  for  enhanced  mode,  and  the  system  is  using  an  enhanced  graphics  adapter  monitor. 
The  program  will  automatically  set  the  graphics  adapter  card  to  its  highest  setting  which 
in  this  case  is  VGA;  this  results  in  the  monitor  receiving  incorrect  graphic  information 
and  will  cause  screen  disruption  or  system  lockup.  Therefore,  the  graphics  program  should 
not  be  used  in  situations  like  this.  The  function  tests  for  all  common  graphics  cards.  If 
no  graphics  card  is  present,  the  program  informs  the  user  that  a  graphics  card  is  necessary 
and  then  terminates. 
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Figure  26.  Typical  Output  of  Graphics  Program 


IV.  ASSOCIATED  AUV  HARDWARE 


The  hardware  development  and  designs  presented  in  this  thesis  are  by  no  means  all 
inclusive  of  the  AUV  project.  The  AUV  project  has  been  distributed  hardware 
responsibilities  among  many  individuals.  The  common  bond  has  been  the  GRiDCASE 
1535  EXP  laptop  computer  and  the  associated  data  translation  board.  The  initial  designs 
of  the  AUV  electronics  called  for  an  intricate  network  of  sensors  and  control  servos. 
These  devices  would  be  sampled  and  sent  control  signals,  respectively,  by  the  Grid 
interface  network.  This  network  consists  of  signal  conditioning  modules,  power  supplies, 
and  sensors.  A  portion  of  the  network  is  depicted  in  Figure  27. 

The  Grid  computer  was  to  initially  contain  all  of  the  code  for  control,  local  path¬ 
planning,  and  mission-commander.  This  basic  premise  has  grown  to  the  current  design 
which  calls  for  a  two  computers,  the  Grid  and  a  Gespack.  The  Gespack  computer  is  a 
rack-based  system  which  runs  the  OS9  operating  system.  It  contains  a  separate  data 
translation  board  from  that  of  the  Grid  and  is  designed  to  operate  both  autonomously  or 
networked.  In  this  scheme  the  Grid  acts  as  a  quasi-Lisp-machine,  since  it  runs  a  Lisp 
interpreter  while  not  being  a  true  Lisp-machine.  It  contains  the  Lisp  code  written  for  the 
global  path-planner  while  the  Gespack  runs  the  control  code  for  AUV  mission.  This 
modified  hardware  scheme  still  requires  an  elaborate  interface  network  for  signal 
conditioning.  The  following  sections  deal  with  two  different  aspects  of  the  signal 
conditioning  problem,  filtering  and  synchro  conversion  of  the  rate  gyro  signals. 
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A.  LOWPASS  ACTIVE  ANALOG  FILTER 


This  lowpass  active  filter  was  designed  to  be  used  as  an  anti-aliasing  filter.  Its 
relative  position  in  the  signal  interface  network  is  shown  in  Figure  27.  In  order  to 
determine  the  filter  cutoff  frequency  fe,  the  open-loop  bandwidth  of  the  dive  control 
system  had  to  be  determined  along  with  the  sampling  frequency.  The  sampling  frequency 
for  the  AUV  prototype  is  proposed  to  be  20  Hz.  This  results  in  a  significant  oversampling 
ratio  which  is  discussed  in  more  detail  later  in  this  section.  In  order  to  determine  the 
open-loop  bandwidth  of  the  dive  control  system,  the  transfer  function  or  state  space  model 
for  that  system  had  to  be  derived.  The  open-loop  Bode  plot  could  then  be  plotted  and  the 
bandwidth  determined. 

The  discrete-time  transfer  function  for  the  dive  system  was  developed  by  Schwartz 
[Ref.  3J.  The  corresponding  continuous-time  dynamics  were  derived  and  estimated  by 
Davis  [Ref.  7],  and  its  transfer  function  has  been  estimated  to  be 
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in  state-space  format  where  q,  0,  and  z  are  defined  in  Table  1,  and  the  input  u  is  the  dive 
command  signal.  This  transfer  function  is  based  on  a  linearized  model  operating  at  300 
RPM.  With  the  state  space  model  defined,  an  open-loop  Bode  plot,  magnitude  only,  was 
then  produced.  The  code,  auvbode.m,  used  to  produce  this  plot  is  located  in  Appendix  A. 
The  Bode  plot  is  depicted  in  Figure  28;  it  represents  the  dive  system  dynamics  and  gives 
us  a  numerical  value  for  the  system  bandwidth.  The  Bode  diagram  demonstrates  the  low 
frequency  nature  of  the  AUV  dive  system.  Because  the  system  bandwidth  is 
approximately  0.2  Hz  and  the  sampling  frequency  is  20  Hz,  the  system  is  being 
significantly  oversampled.  This  simplifies  many  associated  sampling  problems  and  insures 
that  the  Nyquist  criterion 

/,  >  2/^  (4-3) 

is  satisfied  [Ref.  8].  In  determination  of  the  cutoff  frequency  fc  needed  for  the  anti¬ 
aliasing  filter,  the  bandwidth  was  rounded  up  to  0.5  Hz.  This  fact,  coupled  with  the  20 
Hz  sampling  frequency  /„  meant  that  the  lowpass  filter  /c  was  required  to  be  within  the 
range  of  5  Hz  to  15  Hz.  This  is  due  to  the  fact  that  the  next  harmonic  of  the  system  will 
occur  at  a  frequency  equivalent  to  the  sampling  frequency  f%  of  20  Hz.  Based  on  these 
facts,  the  lowpass  fc  was  set  at  10  Hz.  Also  due  to  the  high  sampling  frequency  with 
respect  to  the  system  bandwidth,  a  reconstruction  filter  associated  with  the  anti-aliasing 
filter  was  not  needed  [Ref.  8]. 
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Figure  28. 


1.  Numerical  Development  of  Active  Lowpass  Filter 


The  active-resistor-capacitor  filter,  active-RC,  development  is  based  on  the 
generalized-immittance  converter  (GIC)  circuit.  This  is  due  to  the  fact  that  active-RC 
filters  can  be  derived  directly  from  passive-RLC  prototype  networks  by  replacing  the 
passive  inductors  with  active,  GlC-simulated  inductors  as  shown  by  Ghausi  and  Laker  in 
Ref.  9.  The  development  of  the  basic  transfer  function  for  the  active-RC  filter  using  a 
biquadratic  GIC  scheme  is  also  shown  in  Ref.  9.  From  this  basic  derivation,  the  generic 
lowpass  filter  transfer  function  can  be  derived.  This  is  the  basis  of  the  analytical  and 
numerical  development  of  the  lowpass  active-RC  filter  designed.  The  determination  of  the 
appropriate  values  for  the  circuit  components  was  based  on 
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where  fc  is  the  cutoff  frequency  and 

co,  =  2rr  xlO6  (4-5) 

is  the  gain  bandwidth  product.  In  addition  to  these  parameters,  the  following  design 
specifications  were  considered:  maximally  flat  Butterworth  filter  (i.e.,  Q  =  0.707),  /c 
=  10  Hz,  and  negligible  phase  shift  in  the  frequency  region  of  interest 


66 


To  limit  the  amount  of  noise  introduced  by  this  circuit,  the  resistor  values  were 
limited  to  100  kft.  Therefore;  solving  Equation  (4-4)  for  the  capacitor,  a  value  of  0.159 
pF  was  found.  The  complete  schematic  circuit  diagram  is  located  in  Appendix  E.  Due  to 
the  fast  sampling  rate,  a  second-order  Butterworth  filter  with  a  slow  roll-off  was  designed. 
This  circuit  is  a  monatomic  design;  that  is,  for  each  sampled  sensor  a  lowpass  anti¬ 
aliasing  filter  exists  as  shown  in  Figure  27. 

2.  Evaluation  of  Numerical  Results 

Based  on  the  results  of  the  previous  section,  a  numerical  transfer  function  was 
determined.  This  transfer  function  was  based  on  the  generic  lowpass  filter  function 
derived  in  Ref.  3.  Using  this  transfer  function  the  Bode  diagrams  representing  the 
magnitude  and  phase  of  the  designed  lowpass  filter  were  obtained  and  are  depicted  in 
Figures  4-2  and  4-3,  respectively.  The  magnitude  graph  shows  that,  for  an  ideal  op  amp, 
the  circuit’s  3-dB  frequency  is  10  Hz,  as  called  for  in  the  specifications.  While  the  phase 
plot  shows  that  in  the  frequency  region  of  interest  (i.e.,  0.2  Hz),  a  negligible  phase  shift 
exists,  as  desired. 

The  experimental  results  were  well  within  a  tolerable  deviation  from  the  ideal 
results.  These  results  were  also  supported  by  simulation  using  Spice,  a  computer  circuit 
simulation  program.  Due  to  the  very  low  frequency  involved,  a  GlC-circuit  was  a  logical 
choice.  This  is  due  to  the  sensitivity  of  the  circuit  which  has  been  proven  in  related 
experimentation  [Ref.  9]. 
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B.  SYNCHRO-TO-RESOLVER  CONVERSION 

In  order  to  determine  the  direction  or  Heading  of  the  AUV,  a  highly  accurate  Flux 
Gate  Gyro  was  purchased.  The  gyro  specifications  are  400  Hz,  26  volt  reference,  with  an 
11.8  volt  line  signal.  Due  to  these  specifications  which  required  higher  voltages  and 
subsequendy  produce  higher  output  voltages  than  are  capable  of  being  handled  by  the 
system  hardware,  a  conversion  scheme  had  to  be  worked  out  so  that  the  line  voltages 
were  within  a  specified  range  (i.e.,  2.0  volts),  but  still  maintained  the  needed  accuracy 
which  is  inherent  in  the  11.8  volt  line  signal.  To  overcome  this  problem,  a  set  of  chips 
was  purchased  which  included  the  following:  reference  isolation  transformer,  signal 
isolation  transformer,  and  analog-to-digital  converter.  The  problem  that  had  to  be  resolved 
was  the  design  of  this  circuit  including  all  of  the  passive  elements  which  are  used  to  set 
various  parameters  on  the  transformer  chips. 

I.  Directional  Gyro  Background 

Directional  gyros  have  two  degrees  of  freedom.  The  first  degree  of  freedom 
referred  to  as  the  inner  gimbal  axis,  allows  the  spin  axis  to  be  maintained  horizontal  with 
respective  to  the  fixed  Earth  reference  coordinate  system.  The  second  degree  of  freedom, 
referred  to  as  the  azimuth  or  outer  gimbal  axis,  detects  and  indicates  movements  of  the 
vehicle  in  azimuth  from  a  heading  reference.  This  reference  in  the  case  of  the  AUV  is 
along  the  x-axis  or  the  surge-axis  as  previously  defined.  Therefore,  it  is  apparent  that  the 
spin  axis  of  the  directional  gyro  must  continue  to  point  in  a  fixed  direction  in  a  horizontal 
plane.  The  term  fixed  direction,  refers  to  a  mode,  slaved,  or  type  of  gyro  which  makes 
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corrections  in  order  to  maintain  a  fixed  orientation  with  respect  to  the  Earth  reference 
frame.  In  order  to  do  this,  most  gyros  require  the  corrections  to  be  made  externally; 
hence,  the  role  of  the  flux  gate  is  revealed.  A  slaved  gyro  is  precessed  in  azimuth  by  an 
external  reference  signal  such  as  a  magnetic  transmitter.  In  this  case  the  magnetic 
transmitter  (flux  gate)  continuously  aligns  the  azimuth  gimbal  to  magnetic  north. 
Therefore,  the  overall  accuracy  of  the  gyro  depends  in  a  large  part  on  the  flux  gate 
output.  We  note  that  spurious  magnetic  anomalies  and  interference  will  have  an  adverse 
affect  on  the  flux  gate  output  and  thus  result  in  inaccurate  headings.  Due  to  this  sensitive 
characteristic,  the  flux  gate  on  the  AUV  has  been  encased  in  a  nonmagnetic  metal  alloy 
known  as  mu-metal.  This  is  intended  to  lessen  the  probability  of  error  induced  by 
magnetic  interference. 

Equipment  associated  with  gyros  are  typically  power  supplies,  and  synchros, 
or  resolvers.  Usually,  these  pieces  of  equipment  axe  internal  to  the  overall  housing  of  the 
gyro.  The  power  supply  performs  an  obvious  task,  but  some  discussion  rn  the  function 
of  the  synchro  and  resolver  is  warranted.  The  synchro  and  resolver  perform  identical 
missions,  that  of  encoding  the  gyro  output  on  a  signal  to  enable  other  equipment  to  have 
access  to  this  information.  The  primary  difference  in  the  two  schemes  is  the  method  of 
encoding  the  direction  information. 

The  rotor  and  stators  in  a  resolver  are  oriented  90  degrees  with  respect  to  each 
other  as  in  Figure  31.  This  electrical  arrangement  results  in  the  following 
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resolver  shaft  angle  equations: 

R  =  Asia  (cot)  (4-6) 

5,  3  =  y4sin  ( wt )  sin  (0)  (4-7) 

54  2  =  4 sin  (of)  cos  (0)  (4-8) 

where  R  represents  the  rotor  excitation  voltage  which  is  the  ac  reference  voltage,  and  0 
is  the  resolver  shaft  angle.  Thus  the  output  voltage  which  appears  across  stator  terminals 
S,-S,  and  S4-S2  contains  the  necessary  angle  information.  This  information  must  ’>e 
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converted  into  a  usable  format  (i.e.,  natural  binary  or  dc),  hence  the  need  for  resolver 
conversion. [Ref.  10] 

The  synchro  rotor-stator  configuration  differs  from  the  above  resolver  format 
in  that  the  orientation  of  the  stators  with  respect  to  each  other  is  120  degrees.  This  is 
depicted  in  Figure  32.  Due  to  this  orientation  of  the  stators,  three  phases  of  shaft  angle 
information  exist  while  in  resolver  format  only  two  phases  exist  as  previously  shown.  The 
resulting  synchro  format  voltage  equations  are: 

Sj  3  =  j4sin  (cat)  sin  (8)  (4-9) 

$3  2  -  j4sin  (cor)  sin  (0  +  120°)  (4-10) 

52_,  =  i4sin  (w?)  sin  (0  +  240°)  (4-11) 

These  equations  represent  the  line  signal  voltage  which  carries  the  shaft  angle 
information.  The  rotor  excitation  voltage  is  the  same  as  that  of  Equation  (4-6);  also  the 
amplitude  of  the  line  voltage  signal  is  the  same  as  the  reference  signal.  This  is  logical 
because  the  ac-reference  signal  acts  as  a  carrier  for  the  line  signal  [Ref.  10].  The  problem 
that  had  to  be  solved  was  how  to  interpret  the  information  contained  on  each  line  signal 
coming  from  the  stators.  The  three-phase  output  produced  by  the  synchro  had  to  be 
converted  to  a  usable  format  for  interface  with  the  computer. 
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Figure  32.  Simple  Synchro  (After  Ref.  7.) 


2.  Synchro  Conversion  Circuit  Design 

The  output  format  of  the  flux-gate  gyro  presented  several  problems  to  the 
system  designers  of  the  AUV.  Some  of  these  problems  included  the  high  line  signal  and 
reference  voltages.  The  data  translation  board  would  only  accept  a  maximum  of  +5  volts 
rms.  The  desired  voltage  was  +2  volts  rms.  Another  consideration  was  how  to  handle  the 
information  presented  in  a  3-phase  output  such  as  the  one  present  in  the  flux-gate  gyro, 
and  lastly  what  type  of  information  format  was  desired.  The  data  translation  board  can 
accept  both  analog  and  digital  inputs.  This  section  deals  with  one  solution  to  the  problem. 
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a.  Component  Selection 

Presented  with  the  format  problem  mentioned  above  and  the  compatible 
output  required  to  interface  with  the  data  translation  board,  the  following  components 
were  selected:  5S70/411,  5S72/26V,  2S80KD,  and  AD767KN.  These  components  are  all 
manufactured  by  Analog  Devices  tc  perform  the  task  of  synchro-to-resolver  conversion. 
The  additional  passive  components  required  in  the  design  are  off-the-shelf  resistors  and 
capacitors. 

The  5S72/26V  is  a  reference-signal-isolation  transformer.  It  takes  the 
reference  signal  used  in  the  rotor  excitation.  Equation  (4-6),  as  a  input.  The  transformer 
then  steps  down  this  voltage  to  the  standard  2  volts  rms  used  by  most  analog-to-digital 
converters.  The  transformer  also  provides  isolation  for  the  electronics  from  the  resolver. 

The  5S70/411  is  a  line-signal-isolation  transformer.  It  accepts  the  output 
of  the  synchro  stators,  Equations  (4-9),  (4-10),  and  (4-11),  as  a  input.  The  5S70/411 
converts  the  three-phase  11.8  volt  signals  into  a  two-phase  2  volt  rms  resolver  format. 
This  is  the  format  required  by  most  analog-to-digital  converters  used  in  control  and  sensor 
applications.  The  5S70/411  is  designed  specifically  for  synchro-to-resolver  conversion  of 
1 1.8  volt  line  signals. 

The  2S80KD  is  a  monolithic  tracking  resolver-to-digital  converter.  This 
converter  uses  a  ratiometric  tracking  conversion  technique  which  provides  continuous 
position  data  without  conversion  delay.  The  ratiometric  conversion  technique  is  based  on 
the  ratio  of  the  resolver  format  voltages  being  equal  to  the  tangent  of  the  shaft  angle. 
Therefore,  the  tracking  converter  is  not  dependent  on  the  absolute  magnitudes  of  the 


signal  input  [Ref.  10].  It  also  allows  the  circuit  designer  to  select  passive  components  to 
define  the  appropriate  bandwidth  and  the  desired  resolution  of  the  converter. 


The  AD767KN  is  a  digital-to-analog  converter.  It  accepts  a  12-bit  binary 
input  and  converts  it  to  a  analog  output.  This  particular  component  was  not  used  in  the 
synchro-to-resolver  conversion  design,  but  can  be  implemented  without  the  need  for 
handshaking  in  the  circuit  design  which  appears  in  Appendix  E.  This  chip  would  be 
needed  only  in  the  event  that  the  binary  input  lines  to  the  data  translation  board  are 
required  for  additional  purposes  other  than  the  directional  gyro  information.  In  this  case 
the  12-bit  binary  output  of  the  2S80KD  would  need  to  be  converted  to  a  analog  signal 
and  input  into  the  data  translation  board  in  this  manner. 

b.  Design  Considerations 

The  passive  component  selection  was  based  on  the  desire  to  achieve  the 
highest  possible  bandwidth  and  resolution.  The  components  selected  were  all  within  5% 
tolerance.  In  reference  to  the  circuit  schematic  found  in  Appendix  E,  the  resistors  R1  and 
R2  alongwith  the  capacitors  Cl  and  C2  makeup  an  HF  filter.  The  purpose  of  this  filter 
is  to  reduce  the  amount  of  noise  present  on  the  signal  inputs  to  the  2S80.  The  values  of 
Cl  and  C2  are  dependent  on  R1  which  is  equal  to  R2.  If  the  value  of  R1  is  selected  based 
on  optimal  value  given  by  the  manufacturer,  the  following  equation  can  be  solved  for  the 
values  of  Cl  and  C2: 


Cl  =  C2  =  - - - 

2nRl 


(4-12) 
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The  resistor  R4  is  a  gain  scaling  resistor  whose  value  is  determined  by 


i 

R4  =  - — —  (-)  Q 

100  x  10'9  3 


(4-13) 


For  resolution  of  16  bits,  the  DC-error  component  is  0.0025.  Components  R3  and  C3  act 
as  a  all-pass  filter.  These  components  introduce  a  phase  shift  in  order  to  offset  any  phase 
shift  introduced  by  the  circuit  at  the  reference  frequency.  The  values  for  these  components 


are  R3  =  100/fcQ  and  C3  >  -  .  The  maximum  tracking  rate  is  set  by  the 

voltage-controlled-oscillator  input  resistor  R6.  This  is  accomplished  by  selecting  the 
desired  maximum  tracking  rate  Tm„  which  is  not  to  exceed  1/16  of  the  reference 
frequency. 


R6 


5.92  x  107 
65,536(7^) 


(4-14) 


The  maximum  tracking  rate  is  the  highest  angular  speed  for  which  the  converter  output 
is  able  to  keep  track  with  the  input.  The  tracking  rate  has  a  direct  relationship  to  the 
reference  frequency  (i.e.,  the  nigher  the  reference  frequency,  the  higher  the  tracking  rate) 
[Ref.  111. 

The  closed-loop  bandwidth  of  the  converter  is  selected  by  adjusting 
capacitors,  C4  and  C5,  and  resistor  R5.  The  bandwidth  of  the  converter  is  a  measure  of 
the  acceptable  variance  of  a  sinusoidal  input  to  the  converter  (i.e.,  a  80  Hz  bandwidth 
converter  will  accept  a  varying  input  sinusoid  <  80  Hz)  [Ref.  11]-  This  implied  that  the 
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converter  for  the  AUV  did  not  require  a  large  bandwidth  because  the  directional  gyro 
line-signal  will  vary  slowly.  While  this  is,  in  fact,  the  case,  the  circuit  was  designed  with 
a  80  Hz  bandwidth  to  allow  for  any  future  modifications  to  the  associated  systems  of  the 
AUV.  The  trade-off  for  the  increased  bandwidth  was  an  increase  in  the  value  of  R5  by 
20%  while  C4  and  C5  decreased  in  value  30%  and  50%  respectively.  The  following 
equations  were  used  in  determining  the  appropriate  values  of  the  bandwidth  components: 

fjtEF  >  (4-15) 


20.2  x  IQ  3 
R6  x  fBW2 


C5  =  5  x  C4 


R5  =  - - -  Q 

2*/a*C5 


(4-16) 


(4-17) 


(4-18) 


Offset  and  bias  current  at  the  integrator  input  of  the  2S80KD  can  cause 
an  additional  error.  This  error  can  be  lessened  or  completely  eliminated  with  the  use  of 
an  offset  bias  circuit.  This  offset  bias  circuit  is  realized  in  the  design  by  placing  a  resistor, 
R8,  at  the  integrator  input  and  tying  its  output  to  a  potentiometer,  R9,  which  is  coupled 
across  the  ±  12  volt  dc  source  .  The  values  of  these  components,  as  well  as  C6  and  R7 
are  given  in  Reference  12. [Ref.  12] 
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c.  Evaluating  the  2S80KD  Converter  Output 

Many  possible  methods  exist  for  representing  angular  information  in 
digital  form.  The  most  common  method  is  natural  binary  coding.  The  most  significant  bit 
in  this  system  represents  180  degrees,  the  next  90  degrees,  and  so  on.  A  representation 
of  bit  and  angle  equivalence  is  listed  in  Table  2.  This  table  shows  the  accuracy  possible 
with  the  converter  set  in  16  bit  resolution  mode.  In  this  mode  the  accuracy  is 
approximately  19.8  arc  seconds.  The  circuit  can  be  modified  to  represent  the  output  in 
binary  coded  decimal  (BCD)  for  tests  and  evaluation  by  sending  the  output  of  the 
2S80KD  to  a  binary  to  BCD  converter.  The  resulting  output  can  then  be  sent  directly  to 
a  seven-segment  decoder  for  visual  display  purposes. 


Table  2.  NATURAL  BINARY  BIT  CONVERSION  SCHEME 


Bit  Number 

Angle  in  Degrees 

1  MSB 

180.0 

2 

90.0 

3 

45.0 

4 

22.5 

5 

11.25 

6 

5.625 

7 

2.8125 

8 

1.40625 

9 

0.70313 

10 

0.35156 

11 

0.17578 

12 

0.08790 

13 

0.04395 

14 

0.02197 

15 

0.01099 

16 

0.00549 
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V.  CONCLUSIONS 


The  Variable  Structure  Controller  has  been  shown  to  be  robust  and  to  work  under 
adverse  operating  conditions.  This  makes  it  well-suited  for  application  in  the  AUV. 
Simulation  results  have  shown  that,  despite  external  disturbances,  the  system’s  response 
converges  to  a  desired  equilibrium.  In  order  to  preserve  this  property,  a  robust  observer 
was  designed  to  estimate  the  signals  in  the  loop.  This  observer  design  is  based  on  the 
Doyle-Stein  robust  observer  condition. 

The  observer  signals  are  used  in  an  error  detection  algorithm,  which  detects  the 
presence  of  a  sensor  failure.  The  purpose  of  this  is  to  eliminate  the  need  for  redundant 
hardware  systems,  while  still  maintaining  reliable  control  operation  in  case  of  sensor 
failure.  The  simulation  results  show  that  the  autopilot  equipped  with  the  sensor  failure 
detector  we  developed  can  still  maintain  stability  in  the  presence  of  sensor  errors.  This 
is  conceptually  again  a  new  approach  to  an  old  problem  that  of  system  redundancy. 
When  dealing  with  Autonomous  Underwater  Vehicles,  new  methods  for  system 
redundancy  are  required  due  to  space  limitations.  This  autopilot  algorithm  has  proven  to 
be  accurate  and  as  sensitive  to  deviation  in  the  sensor  errors  as  the  user  desires.  The 
inclusion  of  the  error  detection  algorithm  did  not  result  in  any  degradation  in  efficiency 
of  the  overall  autopilot. 

When  converting  the  autopilot  algorithm  to  the  "C"  language,  an  interface  module 
was  necessary  to  link  the  converted  algorithm  to  a  converted  model.  The  model  used  to 
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test  the  real-time  autopilot  had  to  be  modified  to  simulate  the  envisaged  sensor  sampling 
of  the  actual  hardware.  This  modified  model  behaved  identically  to  the  original  "C" 
model  which  declared  all  variables  as  global. 


The  anti-aliasing  filter  design  was  tested  in  a  lab  environment  with  very  satisfactory 
results.  The  very  low  frequency  of  this  filter  and  its  intended  use  called  for  a  special 
design.  The  GIC  active-filter  was  proven  very  well  suited  for  the  application. 

The  software  developed  for  the  autopilot  is  ready  to  be  implemented  on  the  onboard 
computer  of  the  AUV  (Gespack).  Wet-bed  testing  of  the  whole  system  in  various 
maneuvers  is  the  next  step  in  the  development  of  the  autopilot. 
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APPENDIX  A.  HIGH  LEVEL  DEVELOPMENT  SOURCE  CODE 
This  appendix  contains  the  high  level  source  code  used  in  testing  and  evaluation  of 
the  autopilot  algorithm.  The  source  code  is  in  Matlab.  a  "C"-based  development 
language.  This  source  code  was  used  to  produce  all  graphical  representations  presented 
in  this  thesis.  It  is  not  part  of  the  real-time  code  intended  for  the  AUV  prototype  which 
is  written  exclusively  in  "C",  and  is  located  in  Appendix  B.  A  copy  of  this  source  code 
can  be  obtamcd  from  Professor  Roberto  Cristi,  Naval  Postgraduate  School  (see  Initial 
Distribution  List  for  mailing  address). 

Matlab  is  a  registered  trademark  of  The  Math  Works.  Incorporated.  It  does  not 
produce  stand-alone  executable  code,  but  instead  is  an  interactive  program  intended  for 
numerical  problem  solving.  This  program  is  not  available  with  the  source  code  in  this 
appendix.  Matlab  is  required  when  using  the  source  code  listed  in  this  appendix. 
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%  Filename  "auvobs.m" 

%  Doyle  -  Stein  observer  for  continuous  model  of  AUV 
%  This  program  was  used  to  develop  and  evaluate  the  real-time 
%  autopilot  code  for  the  AUV.  It  calls  a  model  that  is  written  in 
%  "C"  language  to  update  the  states  using  the  dynamic  equations 

%  motion  for  the  submersible, 
clear 

ERXP=[ ’Failure  simulated  in  pitch  at  50  (  Sensor  Pitch  =  -35  )’]; 
ERXQ=[’Error  induced  in  pitch  rate  at  5  (  Sensor  Pitch  Rate  =  100000)’]; 
ERXQP=[ ’Sensor  Pitch  Rate  =  100000  at  5,  Sensor  Pitch  =  -35  at  50’]; 
q=input( ’Input  your  q  for  this  run  ’); 
a=.04  ; 
b=.7  ; 

A  =  [  -b  -a  0 
1  0  0 

0  -1  0];  %!!!!!!  notice  the  minus  signs 

%  s=[l;  0.2070;-0.0198]  %  left  real  eigenvector  of  A-B*L 
s=[l;  0.7;  -0.04];  %  left  eigenvector  of  A  for  lambda=0 

B  =  [  -a;0;0];  %  to  get  model  states  to  track  right 

C  =[00  1]; 

D  =0; 

Qe  =  [  .01  0  0 

0  .01  0 

0  0  .01]; 

Re  =  [  1  ]; 

G  =  1; 

qc  =  [  1  0  0 

0  1  0 

0  0  1]; 

rc  =  [10]; 
dt=.25; 

phi=eye(3)+A*dt; 

del=B*dt; 

L=lqr(A,B,qc.rc)  %  returns  optimal  feedback  gains 
Q  =  Qe*l  +  qA2*B*B'; 
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Kd=lqe(A,G,C,Q,Re);  %  returns  optimal  kalman  gains 
Kd=Kd*dt 

%  observer  dynamics: 
phiO=phi-Kd*C; 
ac=poly(phiO) 

km  ax  =400; 
ordered_depth=  150; 
previous=ordered_depth; 
depth=100; 
ipm=500; 
xdot=4.0; 
transient_flag=3; 
thresl=.  001; 
thres2=.0i; 
thres3=.0l; 

abspeed  =  xdot; 
pitchrate=0.0; 
pitch=0.0; 
r=[zeros(l,kmax) 
zeros(l,kmax) 

depth*ones(l,30)  ordered_depth*ones(l,kmax-30)]; 
state  =  zeros(12Jonax); 
xo  =  zeros(3,kmax); 
uk  =  zeros(l.kmax); 
error  =  zeros(3,kmax); 
auv_normalz  =  zeros(l,kmax); 

initial_state  =  [xdot;0;0;0;pitchrate;0;0;0;depth;0;pitch;0]; 
xo(:,l)  =  [pitchrate;pitch;-depth/abspeedj;  %  initialize  observer 
state(:,l)  =  initial_state; 

% . - . main  loop - 

for  j=l:kmax-l 
if  j  >  500, 

ordered_depth=  1 20; 
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end; 

% - Allow  for  Error  Matrix  to  Change  States . % 

if  previous  ~=  ordered_depth, 
transient_flag  =  j+2; 
end 

u=L(3)*ordered_depth/abspeed; 

u=u-L*[state(5,j);state(l  1  ,j);state(9,j)/abspeed]; 

z=state(9,j  )-ordered_depth; 

q=state(5,j); 

th=state(ll,j); 

%  check  the  error  to  the  threshold 
if  abs(error(l,j))  >  thresl, 
p_rate=xo(l  j); 

fprintf(’pitchrate  is  using  obsNn’) 
else 

p_rate=q; 

end 

if  abs(error(2,j))  >  thres2, 
pit  =  xo(2,j); 

fprintf( ’pitch  is  using  obsNn’) 
else 

pit  =  th; 
end 

if  abs(error(3,j))  >  thres3, 
norm_dep_err  =  xo(3  j); 
fprintf( ’normalized  depth  error  is  using  obsNn’) 
else 

norrn_dep_err  =  z/abspeed; 
end 

ex=[p_rate;  pit;  norm_dep_err];  %  setup  to  compute  nonlin 

ste(j)=s’*ex; 

ubar=1.0*(ste(j)); 

if  (abs(ubar)  >  0.4) 
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ubar=0 .4  *s  ign(ste(j ) ) ; 
end 

u=u+ubar; 
divfin  =  u; 
if  (abs(divfin)  >  .4  ) 

divfin  =  0.4*sign(divfin); 
end; 

inputs=[0;  divfin; -divfin;rpm]; 
u  =  divfin; 

state(:,j+ 1  )=model(state(: , j), inputs, dt); 


uk(j+l)=u; 

xo(:,j+l)=  phi*xo(:,j)  +  del*u  ; 

xo(:,j+l)=  xo(:,j+l)  +•  Kd*(z/abspeed  -  C*txo(:,j»); 

abspeed=sqrt((state(l  ,j+l  ))A2+(state(2,j+l  ))A2+(state(3,j+l  ))A2); 

%  —  simulate  a  fault  in  q  (pitchrate) 
if  j>5; 

q= 100000; 

end 

%  —  end  fault 

%  —  simulate  a  fault  in  theta  (pitch) 
if  j>50; 

th=-35.0; 

end 

%  —  end  fault 

%  transient  correction  for  the  observer 

xe(:,  j+1  )=[q;th;z/abspeed]-xo( : ,j ) ; 
if  j>transient_flag, 

error(:,j+l)=ac(l)*xe(:,j+l)+ac(2)*xe(:,j)+ac(3)*xe(:,j-l)+ac(4)*xe(:,j-2); 

end 

previous=ordered_depth; 

end  %  end  of  main  loop 
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%  Begin  plotting  routine 


subplot(211),plot(xe(2,:)),  title(  *  Uncorrected  Pitch  Error’) 

subplot(212),plot(xe(l,:)),  title(’Uncorrected  Pitchrate  Error’) 

%meta  uncorE 

pause 

clg 

axis([0  kmax  -.2  .5]) 
subplot(211),plot([0  kmax], [.05  .05],’:’) 

%text(300,-.  1 4,  ’Threshold  ’); 

?cxt(300,. 05, ’Threshold  \); 
hold  on; 

plot(abs(error(2,:))),  title( ’Corrected  Pitch  Error’) 

%text(.13,.525,ERXP,’sc’); 

hold  off; 

axis([0  kmax  -100  1000]) 
subplot(212),plot([0  kmax], [.005  .005],’:’) 
text(300,. 005, ’Threshold’); 
hold  on; 

plot(abs(error(l,:))),  title( ’Corrected  Pitchrate  Error’) 

text(  .09,  .0 1 5  ,ERXQ,  ’  sc  ’ ) ; 

hold  off; 

meta  errbigq 

pause 

clg 

axis([0  kmax  -.2  .4]); 

subplot(211),plot(abs(error(3,:))),  title(’Depth  Corrected  Error’) 
hold  on; 

plot([0  kmax], [.2  .2],’:’) 
text(300,. 2, ’Threshold’); 
hold  off; 

subplot(212),plot(state(5,:)),  titlefPitch  Rate’) 
text(.l,.03,ERXQP,’sc’); 
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%meta  simderr 
pause 

clg 

axis([0  km  ax  100  160]); 

subplot(211),plot(state(9,:)),title(’  Model  Depth’) 
hold  on; 

plot([0  225  226  kmax],[140  140  120  120],’:’) 
text( 20, 140, ’Ordered  Depth’); 
hold  off; 

axis([0  kmax  -.45  .45]); 
subplot(2 1 2 ),plot(uk) ,title(  ’ Divefin  Action ’ ) 
text(.l,.03,ERXQP,’sc’); 
pause 

%meta  simdd 
clg 

subplot(21  l),plot(state(l  l,:)),title(’Pitch  ’) 
subplot(212),plot(xo(2,:)),title(’  Observer  Pitch  ’) 
pause 
clg 

subplot; 

axis; 

plot( ste ) ,title(  ’  Sliding  Surface ’),grid 
text(.l,.03,ERXQP,’sc’); 


i 


I 
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%  NON-AUAPTIVE  VARIABLE  STRUCTURE  DIVEFIN  CONTROL 
%  filename  =  dauv.m 

%  This  program  is  used  to  determine  the  optimal  feedback  gains  of 
%  the  fullstate  feedback  sliding  mode  contoller.  It  also  test  the 
%  accuracy  of  the  variable  structure  controller. 

[ear; 
a=  .04  ; 
b=.7  ; 

A  =  [  -b  -a  0  %  State  space  model  of  Dive  system 

1  0  0 

0  -1  0];  %!!!!!!  notice  the  minus  signs 
s=[l;  0.2070;-0.0198];  %  left  real  eigenvector  of  A-B*L 
B  =  [  -a;0;0];  %  to  get  model  states  to  track  right 

C  =[00  1]; 
qc  =  [  1  0  0 

0  1  0 

0  0  1],  %  error  covariance  noise 

rc  =  [10];  %  measurement  noise 

dt=25; 

L=lqr(A,B,qc,rc);  %  Optimal  Feedback  gains 

kmax=400; 
depth=0; 
rpm=600; 
xdot=4.0; 

am=A-B*L  ; 

[eigvec,eigval]=eig(am’); 
lambda=real(eigval(  1,1)); 

s=eigvec(:,l)  ;  %  Left  eigenvector  of  dive  system 

st=s’ 

abspeed  =  xdot; 

pitchrate=0.0; 

pitch=0.0; 

state  =  zeros(12,kmax); 
uk  =  zeros(l,kmax); 


auv_normalz  =  zeros(ljkmax); 

initial_state  =  [xdot;0;0;0;pitchrate;0;0;0;depth;0;pitch;0j; 
state(:,l)  =  initial_state; 

% - main  loop - 

i  =  0; 

for  j=l:kmax-l, 

if  (j  <  180), 

ordered_depth=50; 

else, 

ordered_depth=70; 

end 

u  1  =L(  3 )  *ordered_depth/abspee  d; 
u=ul-(L*[state(5,j);state(ll,j);state(9,j)/abspeed]); 
z=state(9,j  )-ordered_depth; 
q=state(5,j); 
th=state(ll,j); 
ex2=[q;th;z/abspeed]; 
ste(j)=s’*ex2; 
ubar=1.0*(ste(j)); 
if  (abs(ubar)  >  0.4) 
ubar=0.4*sign(ste(j)); 
end 

u=u+ubar; 
divfm=  u; 

if  (abs(divfin)  >  .4  )  %  Limit  maximum  deflection  to  .4  radians 

divfm=  0.4*sign(divfin); 

end; 

inputs=[0;divfin;0;rpm]; 
u=  divfin; 

state(:,j+l )=model(state(:,j), inputs, dt);  %  Call  the  dynamic  equations  model 
uk(j+l)=u;  %  Save  u  for  plotting  purposes 

abspeed=rpm/l  50; 

%  abspeed=sqrt((state(l,j+l))A2+(state(2,j+l))A2+(state(3,j+l))A2); 
time(j)  =  dt  *  i; 
timel(j)  =  dt  *  i; 
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i=i+l; 

end 

time(j+l)  =  dt  *  i; 

%  Begin  plotting  routine 
axis([0,70,-l,51]); 

plot(time,state(9,:)),xlabel(’Time  (Sec)’),ylabel(’  Depth  (Feet)’), grid 

%meta  chap2_l 

pause 

axis([0,80,-.2,.4]); 

%plot(time,uk),xlabel(’Time  (Sec)’),ylabel(’  DivFm  Action’), grid 
plot(time,uk),title(’AUV  Run  #1  Dives  of  50  ft  and  70  ft’), 
xlabel(’Time’),ylabel(’  Divfin  ’),grid 
meta  chap3_2 
pause 

axis([0,70,-.08,.l]); 

plot(time,state(5 ,: )),xlabel( ’Time  (Sec) ’),ylabel( ’Pitch  Rate ’ ),grid 

%meta  chap2_3 

pause 

axis([0,70,-.8,.l]); 

plot(time ,state(  1 1 , :  )),xlabel(  ’Time  (Sec)  ’ ) ,y label( ’ Pitch ’ ),grid 

%meta  chap2_4 

pause 

axis([0,70,-.05,.25J); 

plot(timel,ste).  xlabel(’Time  (Sec)’),ylabel(’  Sliding  Surface’), grid 
%meta  chap2_5 
axis; 
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%  Dive  system  BODE  Plot 
%  filename  =  auvbode.m 

%  This  program  determines  the  Bode  plot  of  interest  for 
%  the  AUV  dive  system. 

clear; 

%  Set  initial  model  conditions 
qll  =  1  ; 

q22  =  1  ; 

q33  =  1  ; 

r  =  [0;0;0]; 
dt  =  0.25  ; 

km  ax  =  400  ; 


al=  [  -.7  -.04  0 
1  0  0 
0  -1  0]; 
bl  =  [  -.04 
0 

01; 

q=[ql  1  0  0 

0  422  0 
0  0  q33]; 

c=[0  0  1]; 

[num.den]  =  ss2tf(al,bl,c,0,l) 

w=logspace(-3,3,250); 

[mag,phase]=bode(al  ,bl  ,q,r,  1  ,w ); 
axis([-3,2, -150,50]); 

semilogx(w/(2*pi),20*logl0(mag)),title( ’Magnitude  Response  for  AUV  Dive  Controller’) 
xlabel(’ Frequency  (in  Hertz)’), ylabel(’ Magnitude  (in  dB)’) 
text(.002,-25,’Pitchrate’) 
text(.002,2,’Pitch’) 
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text(.01,25,’Dive') 

grid 

%  Continuous-Time  Nyquist  Plot 
%  filename  q_cnyq.m 

%  This  program  is  used  to  determine  the  effect  of  varying  q  in  the  program 
%  auvobs.m.  This  program  performs  the  nyquist  plot  for  a  family  of  q’s. 

clear; 

clg; 

axis([-l.l  1.1  -1.1  1.1]); 
axisf square’); 
dt=.25; 

%  Plant  definitions 
A=[-.7  -.04  0 
1  0  0 
0  -1  0]; 

B=[-.04;  0;  0]; 

C=[0  0  1]; 

s=[l;  0.7;  -0.04];  °7c  left  eigenvector  of  A  for  lambda=0 
D  =[0]; 

Qe  =  [  .01  0  0 

0  .01  0 

0  0  .01]; 

Re  =  [  1  ]; 

G  =  1; 

qc  =  f  1  0  0 

0  1  0 
0  0  1]; 
rc  =  [10]; 
dt=.25; 

L=lqr(A,B.qc,rc);  %  returns  optimal  feedback  gains 

UR  =  zeros(3);  %  upper  right  block  of  ’Ae’  (A  extended  matrix) 

Be  =  [  B 
0 
0 
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01:  %  B  extended 

Ce  ■=  [0  0  0  L];  %  C  extended 

w=logspace(- 1,1,150); 

tempq=input(’Input  4  values  of  q  as  a  vector  (ie.  [  ])  ’); 

%  Begin  loop  for  family  of  curves 

plot(exp(sqrt(-l)*[0:0. 05:7] 
hold  on; 

titled  ’Continuous  Time  Nyquist  Plot’]), grid; 
for  i  =  1:4, 
q=tempq(i); 
if  i==l, 

QTX1  =  ['—  q=’,num2str(q)]; 
elseif  i==2, 

QTX2  =  [*...  q=',num2str(q)]; 
elseif  i==3, 

QTX3  =  q=',num2str(q)]; 
else. 

QTX4  =  [’  q=’,num2str(q)]; 
end 

Q  =  Qe*l  +  q''2*B*B'; 

Kd=lqe(A,G,C.Q,Re);  %  returns  optimal  kalman  gains 
LL  =  Kd*C;  %  lower  left  block  of  Ae 

LR  =  A-LL-B*L; 

Ae  =  [  A  UR 

LL  LR];  %  A  extended 

[re.imj  =  nyquist(Ae,Be,Ce,D,l,w); 

if  i==l. 

plot(re,im,’--’) 
elseif  i==2, 
plot(re,im,':’) 
elseif  i==3, 
plot(re,im,’-.’) 
else. 
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plot(re,im,’-’) 

end 

end 

hold  off; 

xlabel(’Real’),ylabel( ’Imaginary’);  text(.05,.35,QTXl,’sc’); 

text(.05, . 25, QTX2,’sc’);text(.05,.15,QTX3,’sc’);text(  .05..05, QTX4, ’sc’); 

%  Discrete  Nyquist  Plot  Program  for  AUV  Dive  System 
%  filename  =  q_nyq.m 

%  T.iis  program  is  used  to  determine  the  effect  of  varying  q  in  the  program 
%  auvobs.m.  This  program  performs  the  discrete  Nyquist  plot  for  a  family  of  q’s. 

clear; 

clg; 

axis([-l.l  1.1  -1.1  1.1]); 

axis(’square’); 

dt=.25; 

%  Plant  definitions 
A=[-.7  -.04  0 
1  0  0 

0  -1  0]; 

B=[-.04;  0;  0]; 

C=[0  0  1]; 

s=[l;  0.7;  -0.04];  %  left  eigenvector  of  A  for  lambda=0 

D  =[0]; 

Qe  =  [  .01  0  0 

0  .01  0 

0  0  .01]; 

Re  =  [  1  ]; 

G  =  1; 

qc  =  [  1  0  0 

0  1  0 

0  0  1]; 

rc  =  [10]; 
dt=.  25; 

L=lqr(A,B.qc,rc);  %  returns  optimal  feedback  gains 
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%  upper  right  block  of  ’Ae’  (A  extended  matrix) 


UR  =  zeros(3); 

Be  =  [  B 
0 
0 

0];  %  B  extended 

Ce  =  [0  0  0  L];  %  C  extended 

tempq=input( ’Input  4  values  of  q  as  a  vector  (ie.  [  ])  ’); 

%  Begin  loop  for  family  of  curves 

plot(exp(sqrt(-l  >*[0:0.05:7]), 
hold  on; 

plot([-l  1]»[0  0],’-’); 
for  i  =  1:4, 
q=tempq(i); 
if  i==l, 

QTX1  =  [’—  q=’,num2str(q)]; 
elseif  i==2, 

QTX2  =  [’...  q=’,num2str(q)]; 
elseif  i==3, 

QTX3  =  q=’,num2str(q)]; 
else, 

QTX4  =  [’  _  q=’,num2str(q)]; 
end 

Q  =  Qe*l  +  qA2*B*B’; 

Kd=lqe(A,G,C,Q,Re);  %  returns  optimal  kalman  gains 
LL  =  Kd*C;  %  lower  left  block  of  Ae 

LR  =  A-LL-B*L; 

Ae  =  [  A  UR 

LL  LR];  %  A  extended 

%  Begin  discrete  portion  of  program  (ie.  discretize  Ae  and  Be) 

[PHIe,GAMe]  =  c2d(Ae,Be,dt); 
wr  =  logspace(-1.57,pi,250); 

97 


[mag, phase]  =  dbode(PHIe,GAMe,Ce,D,l,wr); 
for  k=l:length(mag), 

REAL(k)=mag(k)*cos(phase(k)*pi/l  80); 
IMAG(k)=mag(k)*sin(phase(k)*pi/180); 
end 

if  i==l, 

plot(RE  AL.IMAG ,  ’— ’ ) 
elseif  i==2, 

plot(RE  AL,IMAG ,  ’:  ’) 
elseif  i— 3, 

plot(RE  AL,IMAG ,’-.’) 
else, 

plot(RE  AL,1MAG ,  ’  -  ’ ) 
end 
end 

%title(’ Discrete  Nyquist  Plot’) 
xlabei(  ’Real  ’  ),ylabel(  ’Imaginary  ’ ); 
grid; 

text(.05,.35,QTXl,’sc’); 
text  ( .05 , . 25 ,QTX2 ,  ’  sc  ’ ) ; 
text(  .05 , .  1 5  ,QTX3 ,  ’  sc  ’ ); 
text( . 05 ,  .05 ,QTX4,  ’  sc  ’ ); 
hold  off; 
axis(’ normal’); 
axis; 
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%  State  Feedback  Steering  Algorithm 
%  filename  =  auvmd.m 

%  AUV  steering  algorithim  based  on  the  feedback  of  current  direction 
%  and  desired  heading, 
clear 

%K  =  input( ’Input  gain  ’); 
desired_head=input( ’Input  desired  heading  ’); 

K=.3; 

kmax=300; 

depth=0.0; 

head=0; 

rpm=500; 

xdot=4.0; 

dt  =  .25; 

abspeed  =  xdot; 

pitchrate=0.0; 

pitch=0.0; 

yawrate=0.0; 

yaw=0.0; 

state  =  zeros(l  2,kmax); 
ruk  =  zeros(ljkmax); 

initial_state=[xdot;0;0;0;pitchrate;yawrate;0;head;depth;0;pitch;yaw]; 

%  states  are  1234  5  6  7  8  9  10  11  12 

state(:,l)  =  initial_state; 

% . main  loop - 

turn  =  desired_head-state(12,l); 
if  turn  >  pi, 

fprintffTum  is  more  than  180  degrees’) 
desired_head  =  desired_head-(2*pi); 
end 

for  j=l:kmax 

rud  =  -K*(desired_head-state(12,j)); 
rudder  =  rud; 
if  (abs(rudder)  >  .4  ) 
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rudder  =  0.4*sign(rudder); 
end; 

divfin  =  0; 

inputs=[rudder;  divfin; -divfin;rpm]; 
rud  =  rudder; 

state(:  ,j+ 1  )=model(state(:  ,j),  inputs,  dt); 
ruk(j+l)=rud; 

abspeed=sqrt((state(  1  ,j+ 1  ))A2+(state(2,j+l  ))A2+(state(3  ,j+ 1  >^2); 

desired_head; 

state(12,j); 

end  %  end  of  main  loop 

clg 

time=0:kmax; 

subplot(211),plot(time,state(12,:)),title(’  Model  Head’) 
subplot(212),plot(time,ruk),title(’  Rudder  Action’) 
text(.01,.03,’AUVRUD  program  with  gain=.3’,’sc’) 

%meta  pres 
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%  Steering  and  Dive  Simulation  Program  for  the  AUV 
%  filename  =  auv.m 

%  This  program  is  the  final  phase  test  of  the  autopilot  algorithm. 

%  It  links  the  VSC,  Doyle  -  Stein  Observer,  state  feedback  steering,  and 
%  error  detector  components  of  the  controller  for  full  simulation. 

clear 

ERX=[’Error  induced  in  Pitch  Rate  at  time  intervals  of  15  and  130  (p  =  0.0)’]; 
desired_head=input( ’Input  desired  heading  ’); 
ordered_depth=input( ’Input  ordered  depth  ’); 
q  =  5  ; 
a=.04  ; 
b=.7  ; 

A  =  [  -b  -a  0 
1  0  0 

0  -1  0];  %!!!!!!  notice  the  minus  signs 

s=[l;  0.7;  -0.04];  %  left  eigenvector  of  A  for  lambda=0 
B  =  [  -a;0;0];  %  to  get  model  states  to  track  right 

C  =[00  1]; 

D  =0; 

Qe  =  [  .01  0  0 

0  .01  0 

0  0  .01]; 

Re  =  l  1  ]; 

G  =  1; 

qc  =  [  1  0  0 

0  1  0 

0  0  1]; 

rc  =  [10]; 
dt=.25; 

phi=eye(3)+A*dt 

del=B*dt 

L=lqr(A,B,qc,rc);  %  returns  optimal  feedback  gains 
Q  =  Qe*l  +  qA2*B*B’; 
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Kd=lqe(A,G,C,Q,Re);  %  returns  optimal  kalman  gains 
Kd=Kd*dt; 

%  observer  dynamics: 
phiO=phi-Kd*C; 
ac=poly(phiO); 

kmax=400; 
depth=  100; 
rpm=500; 
xdot=4.0; 
thresl=.001; 
thres2=.01; 
thres3=.01; 

K=.3; 

abspeed  =  xdot; 
pitchrate=0.0; 
pitch=0.0; 
yawrate=0.0; 
yaw=0.0; 
r=[zeros(  1  Jcmax) 
zeros(ljkmax) 

depth*ones(l,30)  ordered_depth*ones(  1  ,km ax-30)] ; 
state  =  zeros(124cmax); 
xo  =  zeros(3Jkmax); 
uk  =  zeros(l,kmax); 
ruk  =  zerosdjkmax); 
error  =  zeros(3Jkmax); 
auv_normalz  =  zeros(ljkmax); 

initial_state=[xdot;0;0;0;pitchrate;yawrate;0;0;depth;0;pitch;yaw]; 
%  states  are  1234  5  6  789  10  11  12 

xo(:,l)  =  [pitchrate;pitch;depth/abspeed];  %  initialize  observer 
state(\l)  =  initial_state; 
turn  =  desired_head-state(12,l); 
if  turn  >  pi, 

fprintf(’Tum  is  more  than  180  degrees\n’) 
desired_head  =  desired_head-(2’l<pi); 
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end 
%- — 


main  loop 


for  j=l:kmax-l 


u=L(3)*ordered_depth/abspeed; 
u=u-L*[state(5,j);state(ll,j);state(9j)/abspeed]; 
z=state(9  ,j  )-ordered_depth ; 
q=state(5,j); 
th=state(ll,j); 

%  check  the  error  to  the  threshold 
if  abs(error(l  ,j))  >  thresl, 
p_rate=xo(l,j); 

fprintffpitchrate  is  using  obs\n’) 
else 

p_rate=q; 

end 

if  abs(error(2,j))  >  thres2, 
pit  =  xo(2,j); 

fprintf( ’pitch  is  using  obsVi’) 
else 

pit  =  th; 
end 

if  abs(error(3,j))  >  thres3, 
norm_dep_err  =  xo(3,j); 
fprintf( ’normalized  depth  error  is  using  obsNn’) 
else 

norm_dep_err  =  z/abspeed; 
end 

ex=[p_rate;  pit;  norm_dep_err];  %  setup  to  compute  nonlin 

ste(j)=s’*ex; 

ubar=1.0*(ste(j)); 

if  (abs(ubar)  >  0.4) 
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ubar=0.4*sign(ste(j)); 

end 

u=u+ubar; 
divfin  =  u; 
if  (abs(divfin)  >  .4  ) 

divfin  =  0.4*sign(divfin); 
end; 

u  =  divfin; 

xo(:,j+l)=  phi*xo(:,j)  +  del*u  ; 

xo(:,j+l)=  xo(:,j+l)  +  Kd*(z/abspeed  -  C*(xo(:j))); 

%  simulate  a  fault  in  q 
if  j>15  &  j<130, 
q=00; 
end 

%  end  fault 

%  transient  correction  for  the  observer 
xe( : ,  j+ 1  )= [q;  th;  z/abspeed]  -xo( :  ,j); 

if  j>3, 

error(:  j+l)=ac(l)*xe(:,j+l)+ac(2)*xe(:,j)+ac(3)*xe(:,j-l)+ac(4)*xe(:,j-2); 
end 

%  Simple  AUV  steering  algorithim 

rud  =  -K*(desired_head-state(  1 2 , j)) ; 
rudder  =  rud; 
if  (abs(rudder)  >  .4  ) 

rudder  =  0.4*sign(rudder); 
end; 

inputs=[rudder;  divfin;-divfin;rpm]; 
rud  =  rudder; 

state(:  j+ 1  )=model(state(: , j), inputs, dt); 

ruk(j+l  )=rud; 
uk(j+l)=u; 
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abspeed=sqrt((state(l  j+l))A2+(state(2,j+l))A2+(state(3,j+l))A2); 
end  %  end  of  main  loop 

%  Begin  plotting  portion 
clg 

subplot(211),plot(error(2,:)),  title(’Pitch  Corrected  Error’) 
subplot(212),plot(error(l,:)),  title(’Pitch  Rate  Corrected  Error’) 
text(.03,.04.ERX,’sc’); 

%meta  pres 

pause 

clg 

subplot(211),plot(error(3,:)),  title(’Depth  Corrected  Error’) 
subplot(2 1 2),plot(state(5,: )),title( ’Pitch  Rate  ’ ) 
text(.03,.04,ERX,’sc’); 

%meta  pres 

pause 

clg 

subplot(211),plot(state(9,:)),title(’  Model  Depth’) 
subplot(212),plot(uk),title(’  Divefin  Action’) 
text(.03,.04,ERX,  ’sc  ’); 

%meta  pres 

%  subplot(223),plot(state(5,:)),title(’Pitch  Rate’) 

%  subplot(224),plot(xo(l,:)),title(’  Observer  Pitch  Rate  ’) 
pause 
clg 

%  subplot(221),plot(xo(3,:)),title(’  Observer  Depth,  etc’) 

%  subplot(222),plot(ste),title(’  Sliding  Surface’) 

%  subplot(223),plot(state(l l,:)),title( ’Pitch’) 

%  subplot(224),plot(xo(2,:)),title(’  Observer  Pitch  ’) 
subplot; 

plot(ste),title(’  Sliding  Surface’), grid 
text(.03,.04,ERX,’sc’);  pause.clg 
subplot(21  l),plot(state(12,:)),title(’  Model  Head’) 
subplot(212),plot(ruk),title(’  Rudder  Action’) 
text(.01,.03,’AUVRUD  program  with  gain=.3’,’sc’) 
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%  filename  =  giclp.m 

%  This  file  plots  the  magnitude  portion  of  the  gic  bandpass  active  filter. 

%  This  was  used  in  the  determination  of  the  required  components  of  the 
%  active-RC  filter  discussed  in  Chapter  4. 

%  definitions 
clear; 

R=100e3;  %  resistor  values 

G=l/R; 

wt=2*pi*le6;  %  gain  bandwidth  product 

% . start  loop - 

QP=.707;  %  for  maximally  flat  response 

rq=G/QP; 

C=.159e-6;  %  Capcitor  value 

wc=l/(R*C); 

%— . set  up  transfer  function - 

numl=(2*C*GA2)/w'. 
num2=(2*GA3+2*rq*GA2)/wt; 
num3=2*GA3; 
num=[numl  num2  num3]; 
denl=2*CA2*G/wtA2; 

den2=(2  *G  *CA2)/wt+(4*GA2  *C)/wt  A2+(2  *G*C*rq)/wtA2; 

den3=CA2*G+2*GA3/wtA2+2*GA2*rq/wtA2+4*GA2*C/wt+2*G*C*rq/wt; 

den4=2*GA3/wt+2*GA2*rq/wt+G*C*rq; 

den5=GA3; 

den=[denl  den2  den3  den4  den5]; 

w=logspace(- 1 ,4,250); 

[mag,phase]=bode(num,den,w); 

%  Begin  plotting  routine 
axis([-l,2,-2,7]); 

semilogx(w/(2*pi),20*logl0(mag)),title(’Magnitude  Response  (LPF)  Fc=10  Hz’) 
xlabel( ’Frequency  (in  Hertz)’), ylabel( ’Magnitude  (in  dB)’),grid 
text(.80,.03,’Q  =  .707’, ’sc’); 
text(.05,.03,’Wt=  6.18e6’,’sc’) 
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pause 

axis([-l,3,-180,0]); 

semilogx(w/(2*pi), phase ) 

title( ’Phase  Response  (LPF)  Fc=10  Hz’) 

xlabel( ’Frequency  (in  Hertz)’), ylabel( ’Phase’) 

grid 
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APPENDIX  B.  REAL-TIME  CONTROL  CODE 


This  appendix  contains  the  real-time  autopilot  code  written  in  the  "C"  language. 
It  has  the  following  components:  Variable-Structure  Controller,  Doyle-Stein  Observer, 
error  detector,  and  state  feedback  steering  algorithms.  This  code  is  ready  for 
implementation  in  the  AUV.  Also  included  in  this  appendix  are  two  external  functions 
called  by  the  control  routine,  signum.c  and  matmul.c. 

Particular  attention  should  be  given  to  the  comments  preceding  the  code  and  to  the 
disclaimer.  This  code  was  written  and  compiled  in  Microsoft  C  5.1.  but  is  compiler 
independent  (i.e.,  will  compile  on  any  ANSI  standard  C  or  K&R  C  compiler).  A  copy 
of  this  source  code  is  available  through  Professor  Roberto  Cristi,  Naval  Postgraduate 
School  (see  Initial  Distribution  List). 
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/* 

AUV  Autopilot  Control  Program  Dave  Riling  /  Prof.  Cristi 
This  program  makes  use  of  a  Non-Adaptive  Variable  Structure  Algorithim 
with  an  associated  Doyle-Stein  Observer  used  for  error  checking  in  the 
diving  mechanism.  For  the  steering  a  simply  Single  State  Feedback  control 
scheme  is  used. 

The  following  values  were  used  in  the  determination  of  the  (1)  feedback 
gains,  (2)  observer  gains,  (3)  eigenvalues  and  (4)  transient  response 
coefficents  : 


A  =  [  -.7  -.04  0 

A  matrix 

B  =  [  -.04 

B  matrix 

1  0 

0 

0 

0  -1 

0] 

0] 

C  =  [  0  0  1 

]  C  matrix 

D  =  [  0  ] 

D  matrix 

The  associated  matlab  program  which  was  used  to  evaluate  the  above 
values  has  the  filename  :  "  auvobs.m  ", 

The  following  values  were  obtained  from  the  above  program  : 

(1)  L  =  [  -3.6308  -2.8032  0.3162  ] 

(2)  K  =  [  -0.0036  -0.0679  0.1859  ] 

(3)  S  =  [  1.0 

0.7 

-0.04  J 

(4)  Ac  =  [1.0  -2.6391  2.3302  -0.6874  ] 

The  threshold  levels  used  in  the  sensor  error  detection  algorithim  were 

detennined  using  a  hueristically  graphic  approach. 

threshold_l  =  .003  pitch  rate  threshold  error 

threshold_2  =  .05  pitch  threshold  error 

threshold_3  =  .2  normalized  depth  threshold  error 

The  feedback  gain  used  in  the  steering  algorithim  is  "  Ks  =  .175  ".  This 
value  was  determined  using  a  hueristic  approach. 
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DISCLAIMER:  Once  the  actual  AUV  model  has  been  determined  the  matlab 
file  "  auvobs.m  "  should  be  changed  and  run  to  obtain  the  true  values 
required  for  the  actual  AUV.  Note  this  algorithim  is  designed  to  be 
a  Real  Time  Implementation  and  is  very  Robust;  as  such,  it  should  not 
require  any  changes  other  than  the  above  parmeters  if  that. 

State  Definitions 
mstate[0J  =  xdot 
mstate[l]  =  ydot 
mstate[2]  =  zdot 
mstate[3]  =  rollrate 
mstate[4]  =  pitchrate 
mstate[5]  =  yawrate 
mstate[6J  =  x 
mstate[7]  =  y 
mstate[8]  =  z 
mstate[9]  =  roll 
mstate[10]  =  pitch 
mstate[l  1]  =  yaw 

*/ 

#include  <math.h> 

#include  <stdio.h> 

#include  "auv.h” 


#define  RUD_STOPS  0.4 
#defi ne  PI  3.141593 

#define  TWO_PI  6.283186 
#define  YES  1 

#define  NO  0 


static  double 

ex[]  =(0.0.0.0.0.0}. 
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L[]  =  {  -3.6308,  -2.8032,  0.3162  }, 

state[]  =  {  0.0,  0.0,  0.0  }; 


void  control8(  double  *mstate,  double  *inputs, 

double  ordered_depth,  double  desired_head,  double  rpm  ) 

{ 

int  order_change; 

jump_over  =  0; 
double  bowplane, 
stemplane, 
rudder; 
double  q, 
theta, 
divres, 
obsres, 
abspeed, 
errz, 
divfinl, 
divfin, 

required_tum, 

Ks  =  0.175; 
static  double 

old_ordered_depth  =  0.0; 

/*  Now  read  speed  sensor  */ 

abspeed  =  mstate[0]; 

q  =  state[0]  =  mstate[4]; 

theta  =  state[l]  =  mstate[10]; 

state[2]  =  mstate[8]/abspeed; 

divfinl  =  L[2]  *  ordered_depth/abspeed; 

mat_mul(L,  1,  3,  state,  3,  1,  (Matrix )&divfin); 

divres  =  divfinl  -  divfin; 

if  (ordered_depth  !=  old_ordered_depth)  { 

ordered_change  =  YES;  /*  set  flag  to  indicate  a  change  in  ordered_depth)  */ 

I 
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else  { 

ordered_change  =  NO; 

} 

old_ordered_depth  =  ordered_depth; 

errz  =  (mstate[8]  -  ordered_depth)/abspeed; 

ex[0]  =  instate  [4]; 

ex[l]  =  mstate[10]; 

ex[2]  =  em; 


/* —  Call  Diveplane  Function  — */ 
diveplane(ex,  &divres); 

if  (observer(q,  theta,  errz,  divres,  order_change,  &obsres))  { 
stemplane  =  obsres; 
bowplane  =  -stemplane; 

} 

else  1 

stemplane  =  divres; 
bowplane  =  -divres; 


/*- . start  steering  algorithm- . */ 

if  (desired_head  >  TWO_PI)  { 

printf("  Error  in  desired  heading  coming  from  mission  commanderW); 

printf("  AUV  surfacing  and  slowing  to  150  RPM,  rudder  0  deflectionV); 

stemplane  =  RUD_STOPS; 

bowplane  =  -RUD_STOPS; 

rpm  =  150; 

rudder  =  0; 

jump_over  =  1; 

1 

if  (instate^1]  >  TWO_PI)  { 

printfC  Error  in  directional  gyroVi"); 

printfC  AUV  surfacing  and  slowing  to  150  RPM,  rudder  0  deflectionV); 
stemplane  =  RUD_STOPS; 
bowplane  =  -RUD_STOPS; 
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rpm  =  150; 
rudder  =  0; 
jump_over  =  1; 

} 

if  (jump_over  !=  1)  { 

required_tum  =  desired_head  -  mstate[ll];  /*  Make  the  shortest  turn  */ 
if  (required_tum  >  PI)  { 

desired_head  =  desired_head  -  (2*PI); 

} 

rudder  =  -Ks  *  (desired_head  -  mstate[ll]); 
if  (fabs(rudder)  >  RUD_STOPS) 

rudder  =  RUD_STOPS  *  sign(rudder); 

} 

/*— movedata  ????????????????- . */ 

inputs  [0]  =  rudder; 
inputs[l]  =  stemplane; 
inputs[2]  =  bowplane; 
inputs[3J  =  rpm; 


/*********************************************************************** 
3|<  9|<  3|C 

This  function  determines  the  observer  states  and  also  performs 
error  &  threshold  comparisons  to  check  sensor  validity.  The  arguments 
are  :  pitchrate,  pitch,  normalized  error  in  depth,  current  divefin 
value,  state  of  order_change,  and  observer  divefin  value 

*/ 

int  observer! double  q,  double  theta,  double  errz, 

double  divres,  int  order_change,  double  *obsres) 

I 

#define  No_Error  0 

#define  Sensor_Error  1 

#define  Major_Error  5 

int  i,  flag; 
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static  int 


transient_hold  =  3; 

j  =  0; 


double 

PHIQ  = 

{  0.8250,  -0.01,  0.0, 

0.2500,  1.00,  0.0, 

0.0,  -0.25,  1.0  }; 

double 

DEL[]  =  (  -0.01,  0.0,  0.0  }, 

C[]  =  {  O.o,  0.0,  1.0  }, 

K[]  =  {  -0.0036,  -0.0679,  0.1859  }, 
ErrorO  =  {  0.0,  0.0,  0.0); 

static  double 


obserr[]  =  {  0.0, 

0.0, 

0.0  }, 

obslerr[]  =  {  0.0, 

0.0, 

0.0  ), 

obs2err[]  =  {  0.0, 

0.0, 

0.0  ), 

obs3err[]  =  {  0.0, 

0.0, 

0.0  ), 

old_obst[]  =  {  0.0, 

0.0, 

o.o  ), 

obstate)]  =  {  0.0, 

0.0, 

0.0  ); 

double  cobs, 

comp, 

threshold_l  =  .003,  /*  pitch  rate  threshold  error  */ 

threshold_2  =  .05,  /*  pitch  threshold  error  */ 

threshold_3  =  .2,  /*  normalized  depth  threshold  error  */ 

A1  =  1.0,  /*  transient  error  coefficents  */ 

A2  =  -2.6391, 

A3  =  2.3302, 

A4  =  -0.6874; 


mat_mul(PHI,  3,  3,  old_obst,  3,  1,  obstate); 
DEL[0]  *=  divres; 

DEL[1]  *=  divres; 
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DEL[2]  *=  divres; 


mat_mul(C,  1,  3,  old_obst,  3,  1,  (Matrix  )&cobs); 
comp  =  errz  -  cobs; 
for  (i  =  0;  i  <  3;  i++)  { 

obstate[i]  +=  DEL[i]; 

K[i]  *=  comp; 
obstate[i]  +=  K[i]; 
old_obst[i]  =  obstate[i]; 


} 

/* — Correct  for  Transient  Response — */ 
obserr[0]  =  q  -  obstate[0]; 
obserr[l]  =  theta  -  obstate[l]; 
obserr[2]  =  errz  -  obstate[3]; 
if  (order_change  ==  YES) 
transient_hold=j+3 ; 
if  (j  >  transient_hold)  { 

for  (i  =  0;  i  <  3;  i++)  { 

Error[i]  =  Al*obserr[i]  +  A2*obslerr[i]  +  A3*obs2err[i]  +  A4*obs3err[i]; 

} 


j++; 

for  (i  =  0;  i  <  3;  i++)  { 

obs3err[i]  =  obs2err[i]; 
obs2err[i]  =  obslerr[i]; 
obslerr[i]  =  obserrfi]; 


/*— Compare  the  error  to  the  Threshold  values—*/ 
flag  =  0; 

if  (fabs(Error[0])  >  threshold^ )  ( 

printff  Error[0]  =  %lf  \n",ErTor[0]); 
ex[0]  =  obstate[0]; 
flag  =  Sensor_Error; 
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if  (fabs(Error[l])  >  threshold_2)  { 

printfC  Error[l]  =  %lf  V',Error[l]); 
ex[l]  =  obstatefl]; 
flag  =  Sensor_Error; 

} 

if  (fabs(Error[2])  >  threshold_3)  { 

/*  Big  Problem  depth  sensor  is  malfunctioning  SURFACE  SURFACE  !!  */ 
printf("  Error[2]  =  %lf  \n",Error[2]); 

♦obsres  =  .4;  /*  Full  diveplane  deflection  */ 

return  Major_Error;  /*  Depth  Sensor  error  code  */ 

} 

if  (flag  ==  0)  { 

return  No_Error;  /*  Sensor  readings  appear  normal  do  not  use  observer  */ 

} 

else  { 

diveplane(ex,  &divres); 

♦obsres  =  divres; 
return  Sensor_Error; 

} 

I 

Dive  plane  function  determines  the  appropriate  radian  angle  for  the 
dive  fins.  The  arguments  are  :  ex  =  state  error  vector,  divefin  value 

*1 

void  diveplane(double  *ex,  double  *divres) 

I 

static  double  S[]  =  {  1.0,  0.7,  -0.04  ); 
double  nonlin, 
ste, 

stesign, 

diver, 

dive; 
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diver  =  *divres; 

mat_mul(S,  1,  3,  ex,  3,  1,  (Matrix )&ste); 
nonlin  =  ste; 
stesign  =  sign(ste); 

/♦—Saturation  Switch  for  Nonlinearity - */ 

if  (fabs(nonlin)  >  0.4)  { 
nonlin  =  0.4  *  stesign; 

) 

diver  +=  nonlin; 

dive  =  sign(diver);  /*— Utilize  Signum  Function—*/ 

if  (fabs(diver)  >  RUD_STOPS)  { 
diver  =  RUD_STOPS  *  dive; 

) 

♦divres  =  diver; 
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/* 

*  AUV.H 


* 

* 

★ 

* 

*/ 


Header  file  for  the  AUV  project. 

Associated  Files  :  ’auvs.c’  ’autplt.c’  ’matmul.c’  ’signum.c’ 
’model. c’  ’modelprm.h’ 


typedef  double  “"Matrix; 

void  mat_mul(Matrix  Ml,  int  rl,  int  cl,  Matrix  M2,  int  r2,  int  c2,  Matrix  M3); 
void  control8(  double  “"instate,  double  “"inputs, 

double  ordered_depth,  double  desired_head,  double  rpm  ); 
int  model(  double  “"oldstate,  double  *inputs,  double  *dt,  double  “"instate  ); 
double  sign(  double  argument  ); 
int  observer(double  q,  double  theta,  double  errz, 

double  divres,  int  order_change,  double  *obsres); 
void  diveplane (double  *ex,  double  *divres); 


/* 

* 

* 

* 

* 

*/ 


signum.c 

This  is  a  generic  signum  function  for  use  in  the  A.U.V.  project. 

The  function  accepts  an  argument,  determines  the  sign  of  the  argument, 
and  returns  the  sign  (+1  or  -1). 


double  sign(  double  argument  ) 

{ 

return  (argument  >  0.0  ?  1.0  ;  -1.0); 

} 
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mat  mul.c 


r 

* 

*  This  function  multiples  two  matrices  or  vectors  (or  scalar,  matrix  mult.) 

*  and  returns  the  product,  typical  usages: 

*  A=[l  2  3]  B=[4;  5;  6)  then  mat_mul(A,l,3,B,3,l,(Matrix)&scalar_ans) 

*  A=[l  2  3  B=[l  2  3 

*  4  5  6  4  5  6  then  mat_mul(A,3,3,B,3,3,matrix_ans) 

*  7  8  9]  7  8  9] 

*  This  function  is  called  by  autplt.c,  the  autopilot  program  for  the  AUV 

*  project.  Dave  Riling  16  Jan  90 

*/ 

#include  <stdio.h> 

typedef  double  *Matrix; 

/*  Row  major  access  macro  */ 

#define  Xrm(M,row,col,col_len)  (*(M  +(row  *  coMen)  +  col)) 
void  mat_mul(Matrix  Ml,  int  rl,  int  cl.  Matrix  M2,  int  r2,  int  c2,  Matrix  M3) 
{ 

int  i,j,k; 
double  sum; 

/*  Take  this  out  when  code  is  implemented  in  the  auv  */ 
if  (cl  !=r2)  { 

fprintf(stderr,"mat_mul:  matrices  rows/cols  not  compatibleNn"); 
exit(l); 

) 

for  (i=0;  i  <  r  1 ;  i++) 

for  (j=0;  j  <  c2;  j++)  { 
sum  =  0.0; 

for(k=0;  k  <  cl;  k++)  { 

sum  +=  (Xrm(Ml,i,k,cl)  *  Xrm(M2dt,j,c2)); 

) 

Xim(M3,i,j,rl)  =  sum; 
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APPENDIX  C.  INTERFACE  AND  MODEL  PROGRAMS 
Included  in  this  appendix  are  the  interface  module  and  the  dynamic  equations  of 
motion  model  both  written  in  "C"  for  the  AUV.  Also  included  is  the  header  file  for  the 
model.  The  code  in  this  appendix  is  not  complier  specific,  and  a  copy  can  be  obtained 
by  contacting  Professor  Roberto  Cristi,  Naval  Postgraduate  School  (see  Initial  Distribution 
List). 

The  model  in  "C"  calls  one  external  function,  signum.c,  which  is  located  in 
Appendix  B.  This  model  updates  the  12  states  depicted  in  Table  2.  of  this  thesis  using 
the  dynamic  equations  of  motion  modeled  after  the  U.S.  Navy’s  Swimmer  Delivery 
Vehicle.  The  interface  program  calls  th  nodel  after  the  controller  modifies  the  necessary 
control  surface  commands  (i.e.,  Dive  plane,  rudder,  and  RPM). 
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/* 

*  MODEL, C 

*  function  state  =  model(oldstate, inputs, dt) 

* 

*/ 


#include  <math.h> 

#include  <stdio.h> 

#include  "modelprm.h" 

double  sign(double); 

int  model(double  *oldstate,  double  *inputs,  double  *dt,  double  *mstate) 

( 

int  j,  k; 

double  u,  v,  w,  p,  q,  r,  phi,  theta,  psi; 

double  dr,  ds,  db,  rpm,  delt; 

double  mass,  latyaw,  norpit,  re,  termO; 

double  signu,  signn,  eta,  cdO,  ct,  ctl,  eps,  xprop; 

double  ucf[4],  fp[6],  f[12]; 

double  tmpl.  tmp2,  tmp3,  tmp4; 

double  cos_theta,  sin_theta,  tan_theta; 

double  cos_phi,  sin_phi,  cos_psi,  sin_psi; 

u  =  oldstate[0]; 
v  =  oldstate[l]; 
w  =  oldstate[2]; 
p  =  oldstate[3]; 
q  =  oldstate[4]; 
r  =  oldstate[5]; 
phi  =  oldstate[9]; 
theta  =  oldstate[10J; 
psi  =  oldstate[l  1]; 
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dr  =  inputs  [0]; 
ds  =  inputs[l]; 
db  =  inputs[2]; 
rpm  =  inputs[3]; 

delt  =  *dt; 


latyaw  =  norpit  =  0.0; 
mass  =  weight/g; 
re  =  u*l/nu; 

signu  =  sign(u); 
signn  =  sign(ipm); 
if  (fabs(u)  <  xltest) 
u  =  xltest; 
eta  =  0.012*rpm/u; 
re  =  u*l/nu; 

cdO  =  0.00385  +  1.296e-17  *  (re  -  1.2e7)*(re  -  1.2e7); 
ctl  =  0.008*l*l/a0; 
ct  =  ctl*eta*fabs(eta); 

eps  =  -1.0+signn/signu*(sqrt(ct+1.0)-1.0)/(sqrt(ctl+1.0)-1.0); 
xprop  =  cdO*(eta*fabs(eta)  -  1.0); 


/* 

*  calculate  the  drag  force,  integrate  the  drag  over  the  vehicle 

*  integrate  using  a  4  term  gauss  quadrature 
*/ 


for  (k=0;  k<4;  ++k)  { 
tmpl  =  v+g4[k]*r*l; 
tmp2  =  w-g4[k]*q*l; 
ucf[k]  =  sqrt(tmpl1('tmpl  +  tmp2*tmp2); 
if(1.0e-10  <=  ucf[k])  ( 

termO  =  ((rho/2.0)*(cdy*hh[k3*tmpl*tmpl 

+  cdz*br[k]*tmp2*tmp2))  *gk4[4]*l/ucf[k]; 
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latyaw  +=  termO*tmpl; 
norpit  +=  term0*tmp2; 

> 

} 

/* 

*  force  equations 
*/ 

/* 

*  common  sub-expressions 

V 

tmpl  =  (rho/2.0)*l*l; 

tmp2  =  tmpl*l; 

tmp3  =  tmp2*l; 

tmp4  =  tmp3*l; 

cos_theta  =  cos(theta); 

sin_theta  =  sin(theta); 

tan_theta  =  sin_theta/cos_theta; 

cos_phi  =  cos(phi); 

sin_phi  =  sin(phi); 

cos_psi  =  cos(psi); 

sin_psi  =  sin(psi); 

/* 

*  longitudinal  force 
*/ 

fp[0]  =  mass*v*r  -  mass*w*q  +  mass*xg*q*q 
+  mass*xg*r*r  -  mass*yg*p*q  -  mass*zg*p*r 
+  tmp3*(xpp*p*p+xqq*q*q  +  xrr*r*r+xpr*p*r) 

+  tmp2*(xwq*w*q+xvp*v*p+xvr*v*r+u*q*(xqds*ds+xqdb*db)+xrdr*u*r*dr) 
+  tmpl  *(xw*v*v+xww*w*w  +  xvdr*u*v*dr+u*w*(xwds*ds+xwdb*db) 


+  u*u*(xdsds*dsMs+xdbdb*dbMb+xdrdr*dr*dr)) 
-  (weight  -boy)*sin_theta 
+  tmp2*xqdsn*u*q*ds*eps 

+  tmpl*(xwdsn*u*w*ds+xdsdsn*u*u*ds*ds)*eps 
+  tmpl*u*u*xprop; 


/* 

*  lateral  force 
*/ 

fp[l]  =  -mass*u*r  -  mass*xg*p*q  +  mass*yg*r*r  -  mass*zg*q*r  + 
tmp3*(ypq*p*q  +  yqr*q*r)+tmp2*(yp*u*p  + 
yr*u*r  +  yvq*v*q  +  ywp*w*p  +  ywr*w*r)  +  tmpl* 

(yv*u*v  +  yvw*v*w  +ydr*u*u*dr)  -latyaw  +(weight-boy>* 
cos_theta*sin_phi+mass*w*p+mass*yg*p*p; 

/* 

*  normal  force 
V 

fp[2]  =  mass*u*q  -  mass*v*p  -  mass*xg*p*r  -  mass*yg*q*r  + 
mass*zg*p*p  +  mass*zg*q*q  +  tmp3* 

(zpp*p*p+zpr*p*r  +  zrr*r*r)  +  tmp2*(zq*u 
*q+zvp*v*p  +  zvr*v*r)  +tmpl*(zw*u*w  +  zvv*v*v 
+u*u*(zds*ds+zdb*db))-norpit+(weight-boy)*cos_theta*cos_phi 
+tmp2*zqn*u*q*eps  +tmpl  *(zwn*u*w  +zdsn* 
u*u*ds)*eps; 
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/* 

*  roll  force 
*/ 

fp[3]  =  -iz*q*r  +iy*q*r  -ixy*p*r  +iyz*q*q  -iyz*r*r  +ixz*p*q+ 
mass*yg*u*q  -mass*yg*v*p  -mass*zg*w*p+tmp4*(kpq* 
p*q  +  kqr*q*r)  +tmp3*(kp*u*p  +kr*u*r  +  kvq*v*q  + 
kwp*w*p  +  kwr*w*r)  +tmp2*(kv*u*v  +  kvw*v*w)  + 
(yg*weight  -  yb*boy)*cos_theta*cos_phi  -  (zg*weight  - 
zb*boy)*cos_theta*sin_phi  +  tmp3*kpn*u*p*eps+ 
tmp2*u*u*kprop  +mass*zg*u*r; 


/* 

*  pitch  force 
*/ 

fp[4]  =  -ix*p*r  +iz*p*r  +ixy*q*r  -iyz*p*q  -ixz*p*p  +ixz*r*r- 
mass*xg*u*q  +  mass*xg*v*p  +  mass*zg*v*r  -  mass*zg*w*q  + 
tmp4*(mpp*p*p  +mpr*p*r  +mrr*r*r)+tmp3*(mq*u*q  +  mvp*v*p  +  mvr*v*r)  + 
tmp2*(mw*u*w+mvv*v*v+u*u*(mds*ds+mdb*db))+  norpit  -(xg*weight- 
xb*boy)*cos_theta*cos_phi+tmp3*mqn*u*q*eps  + 
tmp2*(mwn*u*w+mdsn*u*u*ds)*eps- 
(zg*weight-zb*boy)*sin_theta; 


/* 

*  yaw  force 
*/ 

fp[5]  =  -iy*p*q  +ix*p*q  +ixy*p*p  -ixy*q*q  +iyz*p*r  -ixz*q*r- 
mass*xg*u*r  +  mass*xg*w*p  -  mass*yg*v*r  +  mass*yg*w*q  + 
tmp4*(npq*p*q  +  nqr*q*r)  +tmp3*(np*u*p+ 
nr*u*r  +  nvq*v*q  +nwp*w*p  +  nwr*w*r)  +tmp2*(nv* 
u*v  +  nvw*v*w  +  ndr*u*u*dr)  -  latyaw  +  (xg*weight  - 
xb*boy)*cos_theta*sin_phi+(yg*weight)*sin_theta 
+tmp2*u*u*nprop-yb*boy*sin_theta; 
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now  compute  the  f(0-5)  functions 


/* 

* 

*/ 


for  (j=0;  j<6;  -H-j) 

for  (ftj]=0.0Tk=0;  k<6;  ++k) 
fjj]  +=  xmminv{j][k]*fp[k]; 


/* 

*  the  last  six  equations  come  from  the  kinematic  relations 
*/ 

/* 

*  inertial  position  rates  f(6-8) 

*/ 


f[6]  =  u*cos_psi*cos_theta  +  v*(cos_psi*sin_theta* 
sin_phi  -  sin_psi*cos_phi)  +  w*(cos_psi*sin_theta* 
cos_phi  +  sin_psi*sin_phi); 

f[7]  =  u*sin_psi*cos_theta  +  v*(sin_psi*sin_theta* 
sinphi  +  cos_psi*cos_phi)  +  w*(sin_psi*sin_theta* 
cos.  -Phi  -  cos_psi*sin_phi); 

f[8]  =  -u*sin_theta  +v*cos_theta*sin_phi  +w*cos_theta*cos_phi; 


/* 

*  euler  angle  rates  f(9- 1 1 ) 

*/ 

f[9]  =  p  +  q*sin_phi*tan_theta  +  r*cos_phi*tan_theta; 

fX  1 0]  =  q*cos_phi  -  r*sin_phi; 

f[ll]  =  q*sin_phi/cos_theta  +  r*cos_phi/cos_theta; 
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first  order  integration 


/* 

* 

*/ 


for  (j=0;  j<  1 2;  j++) 

mstatelj]  =  oldstate|J]  +  d;lt  *  fjj]; 

return  0; 

} 
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modelprni.h 


/* 

* 

* 

*  This  file  contains  all  of  the  parameter  coefficients 

*  used  by  the  file  MODEL.C. 

* 

*/ 


/* 

*  longitudinal  hydrodynamic  coefficients 
*/ 

const  double 

xpp  =  7.0e-3,  xqq  =  -1.5e-2,  xrr  = 

xudot  =  -7.6e-3,  xwq  =  -2.0e-l,  xvp  = 
xqds  =  2.5e-2,  xqdb  =  -2.6e-3,  xrdr  = 
xww  =  1.7e-l,  xvdr  =  1.7e-3,  xwds 

xdsds  =  -1.0e-2,  xdbdb  = -8.0e-3,  xdrdr 
xwdsn  =  3.5e-3,  xdsdsn  =  -1.6e-3; 


4.0e-3,  xpr  =  7.5e-4, 

-3.0e-3,  xvr  =  2.0e-2, 

-1.0e-3,  xvv  =  5.3e-2, 

=  4.6e-2,  xwdb  =  1  .Oe-2, 

=  -1.0e-2,  xqdsn  =  2.0e-3, 


/* 

*  lateral  hydrodynamic  coefficients 
*/ 


const  double 

ypdot  = 

1.2e-4,  yrdot  = 

1.2e-3, 

yvdot  = 

-5.5e-2,  yp  = 

3.0e-3, 

ywP  = 

2.3e-l,  ywr  = 

-1.9e-2, 

ydr  = 

2.7e-2,  cdy  = 

3.5e-l; 

ypq  =  4.0e-3,  yqr  =  -6.5e-3, 

yr  =  3.0e-2,  yvq  =  2.4e-2, 

yv  =  -1.0e-l,  yvw  =  6.8e-2, 


128 


normal  hydrodynamic  coefficients 


/* 

* 

*/ 

const  double 

zqdot  =  -6.8e-3,  zpp  =  1.3e-4,  zpr  =  6.7e-3,  zrr  =  -7.4e-3, 

zwdot  =  -2.4e-l,  zq  =  -1.4e-l,  zvp  =  -4.8e-2,  zvr  =  4.5e-2, 

zw  =  -3.0e-l,  zvv  =  -6.8e-2,  zds  =  -7.3e-2,  zdb  =  -2.6e-2, 

zqn  =  -2.9e-3,  zwn  =  -5.1e-3,  zdsn  =  -1.0e-2,  cdz  =  1.0; 

/* 

*  roll  hydrodynamic  coefficients 
*/ 

const  double 

kpdot  =  -1.0e-3,  krdot  =  -3.4e-5,  kpq  =  -6.9e-5,  kqr  =  1.7e-2, 

kvdot  =  1.3e-4,  kp  =  -l.le-2,  kr  =  -8.4e-4,  kvq  =  -5.1e-3, 

kwp=  -1.3e-4,  kwr  =  1.4e-2,  kv  =  3.1e-3,  kvw  =  -1.9e-l, 

kpn  =  ~5.7e-4,  kdb  =  0.0; 


*  pitch  hydrodynamic  coefficients 
*/ 

const  double 

mqdot  = -1.7e-2,  mpp  =  5.3e-5,  mpr  = 

mwdot  =  -6.8e-3,  mq  =  -6.8e-2,  mvp  = 

mw  =  1.0e-l,  mvv  =  -2.6e-2,  mds  = 
mqn  =  -1.6e-3,  mwn  =  -2.9e-3,  mdsn 


5.0e-3,  mrr  =  -2.9e-3, 
=  1.2e-3,  mvr  =  1.7e-2 

-4.1e-2,  mdb  =  6.9e-3 

=  -5.2e-3; 
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yaw  hydrodynamic  coefficients 


* 

V 

const  double 

npdot  = -3.4e-5,  nrdot  = -3.4e-3,  npq  =  -2.1e-2,  nqr  =  2.7e-3, 

nvdot  =  1.2e-3,  np  =  -8.4e-4,  nr  =  -1.6e-2,  nvq  =  -l.Oe-2, 

nwp  =  -1.7e-2,  nwr  =  7.4e-3,  nv  =  -7.4e-3,  nvw  =  -2.7e-2, 

ndr  =  -1.3e-2; 


/* 

*  mass  characteristics  of  the  flooded  vehicle 
*/ 

const  double 

weight  -  12000.0,  boy  =  12000.0,  vol  =  200.0,  xg  =  0.0, 

yg  =  0.0,  zg  =  0.20,  xb  =  0.0,  zb  =  0.0, 

ix  =  1500.0,  iy  =  10000.0,  iz  =  10000.0,  ixz  =  -10.0, 

iyz  =  -10.0,  ixy  =  -10.0,  yb  =  0.0, 

1=  17.4,  rho  =  1.94,  g=  32.2,  nu  =  8.47e-4, 

aO  =  2.0,  kprop  =  0.0,  nprop  =  0.0,  xltest  =  0.1, 

degrud  =  0.0,  degstn  =  0.0; 

/* 

*  define  length  fractions  for  gauss  quadrature  terms 
*/ 

const  double 

g4[]  =  {  0.069431844,0.330009478,0.669990521,0.930568155  1, 
gk4[]  =  {  0.1739274225687,  0.3260725774312,  0.3260725774312, 
0.1739274225687  }; 
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/* 

*  define  the  breadth  bb  and  height  hh  terms  for  the  integration 
*/ 

const  double 

br[]  =  {  75.7/12.0,  75.7/12.0,  75.7/12.0,  55.08/12.0  }, 
hh[]  =  (  16.38/12.0,  31.85/12.0,  31.85/12.0,  23.76/12.0  }; 


/* 

*  assemble  inverted  mass  matrix 
*/ 

const  double 

xmminv[6][6]  =  { 

{  0.2431e-2,  0.2701e-8,  0.1899e-5,  0.1649e-7, -0.5023e-5,  0.3243e-8  }, 
{  0.2679e-8,  0.1537e-2,  0.5593e-8,  0.421 6e-4,  -0.1479e-7,  0.1057e-4  }, 
{  0.1899e-5,  0.5639e-8,  0.6293e-3,  0.3443e-7,  -0.1049e-4,  0.6770e-8  }, 
{  0.1649e-7,  0.4321e-4,  0.3443e-7,  0.3294e-3,  -0.9106e-7,  -0.1049e-5  }, 
{  -.5023e-5,  -.1491e-7,  -.1049e-4,  -.9106e-7,  0.2773e-4,  -0.1790e-7  }, 

{  0.3243e-8,  0.1057e-4,  0.6769e-8,  -.1052e-5,  -0.1790e-7,  0.6561e-4  ) 

); 
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APPENDIX  D.  FIELD  EVALUATION  GRAPHICS  ROUTINES 

This  appenuM  contains  the  routines  written  for  field  graphical  analysis  of  AUV  run 
data.  These  programs  are  not  intended  to  produce  hardcopy  output,  but  are  merely  for 
on-the-spot  interpretation  of  AUV  run  data.  They  are  user  friendly  and  will  operate  on 
many  different  graphic  systems. 

Particular  attention  should  be  paid  to  the  disclaimer  in  the  comment  sections  of  the 
source  code.  These  programs  are  compiler  dependent.  They  call  many  nonstandard  C 
functions  in  Libraries  specific  to  MICROSOFT  C  5.1  or  QUICKC  2.0.  These  functions 
are  necessary  for  the  graphics  routines.  This  code  must  be  compiled  using  either  of  the 
above  compilers. 

A  copy  of  this  source  code  is  available  through  Professor  Roberto  Cristi,  Naval 
Postgraduate  School  (see  Initial  Distribution  List).  MICROSOFT  C  5.1  and  QUICKC  2.0 
are  registered  trademarks  of  the  Microsoft  Corporation.  The  code  contained  in  the 
libraries  called  by  this  source  code  is  available  through  Microsoft  Coiporation  and  its 
affiliated  vendors.  The  U.S.  Navy  does  not  support  the  use  of  any  particular  C  compiler; 
C  compiler  used  was  solely  the  preference  of  the  author. 
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This  program  takes  a  columnized  data  file  (x,  y)  and  plots  each  column 
with  respect  to  discrete  time.  It  autoscales  and  labels  the  graph 
appropriately  using  the  tabular  headings  created  in  the  "auvs.c"  program. 
This  file  is  intended  for  field  use  during  AUV  test.  Usage: 

plot  <filename>  ie.  plot  auvrun.dat 
This  program  is  composed  of  :  grph.h  setvid.c. 

The  setvid.c  function  sets  the  program  for  the  correct  graphics  adapter 
installed  (note:  please  see  setvid.c  for  more  information. 

IMPORTANT:  The  first  column  of  the  data  file  must  always  be  time.  This 
column  is  always  the  x-axis  on  the  graphs.  For  a  hardcopy  output  the  only 
facility  is  the  use  of  print  screen,  since  this  program  was  note  intended 
for  hardcopy  output.  There  is  no  limitation  on  the  size  of  the  file  or  the 
number  of  columns  of  data  it  may  contain.  The  larger  the  file  the  longer 
the  required  run  time. 

Final  note:  These  files  have  library  functions  particular  to  Microsoft  5.1 
or  Quick  C  2.0  and  as  such  must  be  compiled  using  either  of  these 
compilers. 

*/ 


#include  "grph.h" 
#include  <malloc.h> 


#define  BUFFER  256 

float  **bigarray; 
struct  text 

I 

char  title[30],  /*  structure  used  for  labeling  graph  */ 

subtitle  [30]; 

|  label; 
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main(  int  argc,  char  **argv  ) 

{ 

chartenv  env;  /*  setup  chart  environment  structure  */ 
int  ij,k,m,n; 

FILE  *fptr; 

char  headingbuf[BUFFER+l], 

databuf[B  UFFER+ 1  ] , 
gridquest[3], 

*label_axis[15]; 
char  *tptr; 

if  ((fptr  =  fopen(argv[l],"rt"))  =  NULL)  | 
perror(argv[l]); 
exit(l); 

} 

else  ( 

fgets  (headingbuf,BUFFER,fptr);  /*  get  the  heading  */ 

n=0; 

tptr  =  headingbuf; 

while  ((strtok  (tptr,"  "))  !=  NULL)  { 
n++; 

tptr  =  NULL; 

I 

m=0; 

while  (fgets  (databuf,BUFFER,fptr)  !=  NULL)  /*  get  the  data  */ 
m++; 

rewind(fptr); 

for  (k=0;  k  <  n;  k++)  { 

fscanf(fptr,”%s",label_axis[k]);  /*  discard  the  headings  */ 

) 

} 

if  ((bigarray  =  calloc  (n,sizeof(float*)))  ==  NULL)  ( 

printf  ("  Something  is  wrong  with  memory  cannot  initialize  arrayW); 
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exit(l); 


} 

for  (i=0;  i  <  n;  i++)  { 

if  ((bigarray[i]  =  caIloc(m,sizeof(float)))  =  NULL)  { 

printf  ("  Check  memory  cannot  load  data  for  arrayW); 
exittl); 

} 

} 

/*  Begin  graphics  set  up  */ 
for  (i=0;  i  <  m;  i++)  { 

for  (j=0;  j  <  n;  j++)  { 

fscanf  (fptr,"%f',&bigarray{j][i]); 

} 


puts(" Enter  title  of  the  graph  (  limit  30  characters  )"); 
gets(label.title); 

putsC'Enter  the  subtitle  of  the  graph  (  limit  30  characters  )"); 
gets(label. subtitle); 

/*  Begin  graphics  hardware  interface  */ 
graphics_mode();  /*  call  the  setvid.c  file  */ 

_clearscreen(  _GCLEARSCREEN  ); 

_pg_initchart(); 

_pg_defaultchart(  &env,  _PG_SCATTERCHART,  _PG_POINT ANDLINE  ); 
strcpy(  env.maintitle. title,  label. title  ); 

env.maintitle.titlecolor  =1;  /*  color  1  =  white, text  color  */ 

env.maintitle.justify  =  _PG_CENTER; 
strcpy(  env. subtitle. title,  label. subtitle), 

env. subtitle. titlecolor  =1;  /*  color  1  =  white,text  color  */ 

env.subtitle.justify  =  _PG_CENTER; 
strcpy(  env.xaxis.axistitle.title,  "Time"); 
env.chartwindow.border  =  TRUE; 
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_pg_getpalette(  pal  );  /*  Set  chart  point  value  to  =  blank  */ 

pal[l].plotchar  =  32; 

_pg_setpalette(  pal  ); 
jmpbak: 

puts("Do  you  want  grids  on  your  graph  (y  or  n)?"); 
gets  (gridquest); 
switch  (*gridquest)  { 
case  ’y’: 

env.yaxis.grid  =  TRUE; 
env.xaxis.grid  =  TRUE; 
env.yaxis.gridstyle  =  4; 
env.xaxis.gridstyle  =  5; 

break;  j 

] 

case  V: 

env.yaxis.grid  =  FALSE; 
env.xaxis.grid  =  FALSE; 
break; 
default: 

printf("Try  again.  Input  lowercase  y  or  n  only!  \n"); 
goto  impbak; 

) 

/*  Begin  plotting  values  */ 
for  (i=l;  i  <  n;  i++)  { 

strcpy(  env.yaxis.axistitle. title,  label_axis[i]); 

_pg_chartscatter(  &env,  bigarrayJO],  bigarray[i],  m); 
getch(); 

) 

__setvideomode(  _DEFAULTMODE  );  /*  reset  the  video  back  to  original  */ 

/*  setting  prior  to  running  plot.exe*/ 

for  (i=0;  i  <  n;  i++) 
free(bigarray[i]); 
free(bigarray); 
fclose(fptr); 
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setvid.c 


/* 


This  function  sets  the  video  graphics  mode  for  the  graphing  routine: 
plot.  It  checks  for  the  graphics-adapter  card  that  is  present  in  the 
respective  computer  and  sets  it  automatically  to  its  highest  resolution. 
This  function  is  called  by  "plot.c".  It  returns  a  void  and  accepts  no 
arguments. Graphics  cards  that  are  accepted  by  this  program  are: 

1)  VGA  2)  EGA  3)  CGA  4)  HERCULES. 

DISCLAIMER:  Due  to  the  configuration  of  the  standard  Microsoft  ’C’ 
libraries  used,  this  function  will  always  set  the  graphics  card  present 
to  its  highest  resolution  available  regardless  of  the  monitor  in  use. 
Therefore,  if  you  have  a  VGA  graphics  card  installed  and  are  using  a 
EGA  monitor  the  "plot.exe”  program  will  not  work. 

*/ 

#include  "grph.h" 

void  graphics_mode(  void  ) 

( 

_getvideoconfig(  &myscreen  ); 
switch(  myscreen. adapter  )  { 
case  _CGA: 
case  _OCGA: 

_setvideomode(  _HRESBW  ); 
break; 
case  _EGA: 
case  _OEGA: 

_setvideomode(  _ERESCOLOR  ); 
break; 
case  _VGA: 
case  _OVGA: 
case  _MCGA: 

_setvideomode(  _VRES16COLOR  ); 
break; 
case  _HGC: 

_setvideomode(  _HERCMONO  ); 
break; 
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default. 

printf(  "This  program  requires  a  graphics  card."); 
exit(O); 

} 

_getvideoconfig(  &myscreen  ); 

maxx  =  myscreen.numxpixels  -  1;  /*  this  information  is  not  used  */ 

maxy  =  myscreen.numypixels  -  1;  /*  currently,  it  is  used  for  */ 

}  /*  manual  axis  scaling  */ 


This  is  the  header  file  for  the  program  "graph.exe". 
Associated  files  are  :  ’plot.c’  and  ’servid.c’. 

*/ 

#include  <stdlib.h> 

#include  <stdio.h> 

#include  <conio.h> 

#include  <graph.h> 

#include  <pgchart.h> 

#include  <string.h> 

#include  <math.h> 

typedef  enum  (FALSE,  TRUE)  boolean; 
void  graphics_mode(  void  ); 
struct  videoconfig  myscreen; 
int  maxx,  maxy; 
palettetype  pal; 


APPENDIX  E.  HARDWARE  DESIGN  SCHEMATICS 


The  diagrams  in  this  appendix  pertain  to  the  designs  discussed  in  Chapter  IV.  of  this 

j  ■ 

thesis.  These  diagrams  are  of  the  active-RC  filter  circuit  and  the  Synchro-to-Resolver 
circuit.  For  explanation  and  positioning  of  these  curcuits,  please  refer  to  Chapter  IV. 
These  circuits  are  intended  for  use  in  the  signal  conditioning  network  of  the  AUV.  For 
further  assistance  refer  to  the  technical  notes  listed  in  the  references.  These  notes  are 


available  in  the  AUV  library. 
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